00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include <stdio.h>
00023 #include <math.h>
00024 #include <assert.h>
00025 #include <string.h>
00026 #include <float.h>
00027
00028 #include "vec3d.h"
00029 #include "xform3d.h"
00030 #include "ram.h"
00031 #include "matrix33.h"
00032 #include "quatern.h"
00033
00034 #include "PhysicsObject.h"
00035 #include "PhysicsJoint.h"
00036 #include "PhysicsSystem.h"
00037
00038 typedef struct LinearSystemStruct
00039 {
00040 geFloat ** M;
00041 geFloat * X;
00042 geFloat * KVector;
00043 } LinearSystemStruct;
00044
00045 typedef struct gePhysicsSystem
00046 {
00047 int sumOfConstraintDimensions;
00048 LinearSystemStruct *linsys;
00049 int PhysicsObjectCount;
00050 int PhysicsJointCount;
00051 gePhysicsObject **Objects;
00052 gePhysicsJoint **Joints;
00053 geFloat physicsScale;
00054
00055 int sourceConfigIndex, targetConfigIndex;
00056
00057 } gePhysicsSystem;
00058
00059
00060 static geBoolean gePhysicsSystem_EnforceConstraints(gePhysicsSystem* physsysPtr, geFloat subStepSize);
00061 static geBoolean gePhysicsSystem_SolveForConstraintForces(gePhysicsSystem* physsysPtr);
00062
00063 static Matrix33 gePhysicsSystemIdentityMatrix;
00064
00065 GENESISAPI gePhysicsSystem* GENESISCC gePhysicsSystem_Create(void)
00066 {
00067 gePhysicsSystem* pPhyssys;
00068
00069 pPhyssys = NULL;
00070 pPhyssys = GE_RAM_ALLOCATE_STRUCT(gePhysicsSystem);
00071 if (pPhyssys == NULL)
00072 {
00073 return NULL;
00074 }
00075
00076 memset(pPhyssys, 0, sizeof(*pPhyssys));
00077
00078 Matrix33_SetIdentity(&gePhysicsSystemIdentityMatrix);
00079
00080 pPhyssys->sourceConfigIndex = 0;
00081 pPhyssys->targetConfigIndex = 1;
00082
00083 return pPhyssys;
00084 }
00085
00086 GENESISAPI geBoolean GENESISCC gePhysicsSystem_AddObject(gePhysicsSystem *PS, gePhysicsObject *Object)
00087 {
00088 gePhysicsObject ** NewList;
00089 assert( PS != NULL );
00090
00091 NewList = geRam_Realloc(PS->Objects, sizeof(*NewList) * (PS->PhysicsObjectCount + 1));
00092 if (!NewList)
00093 return GE_FALSE;
00094
00095 NewList[PS->PhysicsObjectCount] = Object;
00096 PS->PhysicsObjectCount++;
00097 PS->Objects = NewList;
00098
00099 return GE_TRUE;
00100 }
00101
00102 GENESISAPI geBoolean GENESISCC gePhysicsSystem_AddJoint(gePhysicsSystem *PS, gePhysicsJoint *Joint)
00103 {
00104 gePhysicsJoint ** NewList;
00105 int i;
00106 gePhysicsJoint_Kind type;
00107 assert( PS != NULL );
00108 assert( Joint != NULL );
00109
00110 NewList = geRam_Realloc(PS->Joints, sizeof(*NewList) * (PS->PhysicsJointCount + 1));
00111 if (!NewList)
00112 return GE_FALSE;
00113
00114 NewList[PS->PhysicsJointCount] = Joint;
00115 PS->PhysicsJointCount++;
00116 PS->Joints = NewList;
00117
00118
00119 if (PS->linsys)
00120 {
00121 assert(PS->linsys->M);
00122 assert(PS->linsys->X);
00123 assert(PS->linsys->KVector);
00124
00125 for (i = 0; i < PS->sumOfConstraintDimensions; i++)
00126 {
00127 assert(PS->linsys->M[i]);
00128 geRam_Free(PS->linsys->M[i]);
00129 }
00130
00131 geRam_Free(PS->linsys->M);
00132 geRam_Free(PS->linsys->X);
00133 geRam_Free(PS->linsys->KVector);
00134 geRam_Free(PS->linsys);
00135 }
00136
00137 PS->linsys = GE_RAM_ALLOCATE_STRUCT(LinearSystemStruct);
00138 if (PS->linsys == NULL)
00139 {
00140 return GE_FALSE;
00141 }
00142
00144
00145
00146 PS->sumOfConstraintDimensions = 0;
00147 for ( i = 0; i < PS->PhysicsJointCount; i++)
00148 {
00149 type = gePhysicsJoint_GetType(NewList[i]);
00150 switch (type)
00151 {
00152 case JT_WORLD:
00153 case JT_SPHERICAL:
00154 PS->sumOfConstraintDimensions += 3;
00155 break;
00156
00157 default:
00158
00159 assert(!"Illegal joint kind");
00160 return GE_FALSE;
00161 }
00162 }
00163
00164 if (PS->sumOfConstraintDimensions == 0)
00165 return GE_FALSE;
00166
00168
00169
00170 PS->linsys->M = (geFloat**)geRam_Allocate(PS->sumOfConstraintDimensions * sizeof(geFloat*));
00171
00172 if (PS->linsys->M == NULL)
00173 {
00174 return GE_FALSE;
00175 }
00176
00177 for (i = 0; i < PS->sumOfConstraintDimensions; i++)
00178 {
00179 PS->linsys->M[i] = (geFloat*)geRam_Allocate(PS->sumOfConstraintDimensions * sizeof(geFloat));
00180
00181 if (PS->linsys->M[i] == NULL)
00182 {
00183 return GE_FALSE;
00184 }
00185 }
00186
00187 PS->linsys->X = (geFloat*)geRam_Allocate(PS->sumOfConstraintDimensions * sizeof(geFloat));
00188
00189 if (PS->linsys->X == NULL)
00190 {
00191 return GE_FALSE;
00192 }
00193
00194 PS->linsys->KVector = (geFloat*)geRam_Allocate(PS->sumOfConstraintDimensions * sizeof(geFloat));
00195
00196 if (PS->linsys->KVector == NULL)
00197 {
00198 return GE_FALSE;
00199 }
00200
00201 return GE_TRUE;
00202 }
00203
00204 GENESISAPI geBoolean GENESISCC gePhysicsSystem_Destroy(gePhysicsSystem** ppPhyssys)
00205 {
00206 gePhysicsSystem *pPhyssys;
00207 int i;
00208
00209 pPhyssys = *ppPhyssys;
00210
00211 geRam_Free(pPhyssys->Objects);
00212 geRam_Free(pPhyssys->Joints);
00213
00214
00215 if (pPhyssys->linsys)
00216 {
00217 assert(pPhyssys->linsys->M);
00218 assert(pPhyssys->linsys->X);
00219 assert(pPhyssys->linsys->KVector);
00220
00221 for (i = 0; i < pPhyssys->sumOfConstraintDimensions; i++)
00222 {
00223 assert(pPhyssys->linsys->M[i]);
00224 geRam_Free(pPhyssys->linsys->M[i]);
00225 }
00226
00227 geRam_Free(pPhyssys->linsys->M);
00228 geRam_Free(pPhyssys->linsys->X);
00229 geRam_Free(pPhyssys->linsys->KVector);
00230 }
00231
00232 geRam_Free(pPhyssys->linsys);
00233 geRam_Free(*ppPhyssys);
00234 *ppPhyssys = NULL;
00235
00236 return GE_TRUE;
00237 }
00238
00240
00241
00242 static geFloat fmin(geFloat a, geFloat b)
00243 {
00244 return a < b ? a : b;
00245 }
00246
00247 GENESISAPI geBoolean GENESISCC gePhysicsSystem_Iterate(gePhysicsSystem* psPtr, geFloat Time)
00248 {
00249 int i;
00250
00251 int numIntegrationSteps;
00252 geFloat minAssemblyRate, subStepSize;
00253 geFloat amountIntegrated = 0.f;
00254
00255 assert( psPtr != NULL );
00256
00258
00259
00260
00261 minAssemblyRate = FLT_MAX;
00262
00263 if (psPtr->PhysicsJointCount == 0)
00264 {
00265 numIntegrationSteps = 1;
00266 }
00267
00268 else
00269 numIntegrationSteps = 5;
00270
00271 if (Time > 0.03f) Time = 0.03f;
00272
00273 subStepSize = Time / numIntegrationSteps;
00274
00275 for ( amountIntegrated = 0.f;
00276 amountIntegrated < Time;
00277 amountIntegrated += subStepSize)
00278 {
00279 for (i = 0; i < psPtr->PhysicsObjectCount; i++)
00280 {
00281 if (!gePhysicsObject_ComputeForces(psPtr->Objects[i], psPtr->sourceConfigIndex))
00282 return GE_FALSE;
00283 }
00284
00286
00287
00288 if (psPtr->sumOfConstraintDimensions > 0)
00289 {
00290 if (!gePhysicsSystem_EnforceConstraints(psPtr, subStepSize))
00291 return GE_FALSE;
00292 }
00293
00294 for (i = 0; i < psPtr->PhysicsObjectCount; i++)
00295 {
00296 if (!gePhysicsObject_Integrate(psPtr->Objects[i], subStepSize, psPtr->sourceConfigIndex))
00297 return GE_FALSE;
00298 }
00299
00300 psPtr->sourceConfigIndex = (psPtr->sourceConfigIndex == 0 ? 1 : 0);
00301 psPtr->targetConfigIndex = (psPtr->targetConfigIndex == 0 ? 1 : 0);
00302
00303
00304 }
00305
00306 for (i = 0; i < psPtr->PhysicsObjectCount; i++)
00307 {
00308 gePhysicsObject* pod;
00309
00310 pod = psPtr->Objects[i];
00311
00312 gePhysicsObject_ClearAppliedForce(pod, psPtr->sourceConfigIndex);
00313 gePhysicsObject_ClearAppliedTorque(pod, psPtr->sourceConfigIndex);
00314 gePhysicsObject_SetActiveConfig(psPtr->Objects[i], psPtr->sourceConfigIndex);
00315 }
00316
00317 return GE_TRUE;
00318 }
00319
00320 static geBoolean gePhysicsSystem_EnforceConstraints(gePhysicsSystem* PS, geFloat subStepSize)
00321 {
00322 geFloat h, hSquared;
00323 geVec3d beta, D0, D1;
00324 geVec3d K;
00325 geVec3d rA, rB;
00326 geVec3d accA, accB;
00327 geVec3d velA, velB;
00328
00329 Matrix33 tmpMat;
00330 Matrix33 term11, term12, term21, term22;
00331 Matrix33 rStarA, itA, itiA, rStarB, itB, itiB;
00332 Matrix33 rotA, rotB, rotAt, rotBt;
00333
00334 geVec3d tmpVec;
00335 geVec3d jntLocA, jntLocB;
00336 geVec3d jntLocAWS, jntLocBWS;
00337 geVec3d offsetVecA, offsetVecB;
00338 geVec3d omegaA, LA, omegaB, LB;
00339 geVec3d zeroForceAccelerationA, zeroForceAccelerationB;
00340 geVec3d ptVelocityA, ptVelocityB;
00341 geVec3d alphaA, alphaB;
00342 geVec3d ptAccelerationA, ptAccelerationB;
00343
00344 geVec3d linearVelocityA, linearVelocityB;
00345 geVec3d angularVelocityA, angularVelocityB;
00346
00347 geXForm3d xformA, xformB;
00348
00349 Matrix33 iTensorA, iTensorB;
00350 Matrix33 iTensorInvA, iTensorInvB;
00351
00352 geVec3d constraintForce;
00353 Matrix33 Mblock;
00354
00355 int i, j, k, iOffset;
00356 int size;
00357
00358 int si;
00359
00360 gePhysicsJoint* jntData;
00361 gePhysicsObject *podA, *podB;
00362
00363 gePhysicsJoint_Kind type;
00364
00366
00367 assert( PS != NULL );
00368
00369 si = PS->sourceConfigIndex;
00370
00371 assert(PS != NULL);
00372
00373 size = PS->sumOfConstraintDimensions;
00374
00375 for (i = 0; i < size; i++)
00376 for (j = 0; j < size; j++)
00377 PS->linsys->M[i][j] = 0.f;
00378
00379
00380 for (i = 0, iOffset = 0; i < PS->PhysicsJointCount; i++)
00381 {
00382 jntData = PS->Joints[i];
00383 h = gePhysicsJoint_GetAssemblyRate(jntData);
00384 hSquared = h * h;
00385
00387
00388
00389 type = gePhysicsJoint_GetType(jntData);
00390
00391 switch(type)
00392 {
00393 case JT_WORLD:
00394
00395 podA = gePhysicsJoint_GetObject1(jntData);
00396
00397 assert(podA != NULL);
00398
00399 gePhysicsObject_GetLocation(podA, &offsetVecA, si);
00400 gePhysicsObject_GetXForm(podA, &xformA, si);
00401 gePhysicsJoint_GetLocationA(jntData, &jntLocA);
00402
00403 geXForm3d_Rotate(&xformA, &jntLocA, &rA);
00404 geVec3d_Add(&offsetVecA, &rA, &tmpVec);
00405 gePhysicsJoint_SetLocationAInWorldSpace(jntData, &tmpVec);
00406
00407 Matrix33_MakeCrossProductMatrix33(&rA, &rStarA);
00408
00409 Matrix33_ExtractFromXForm3d(&xformA, &rotA);
00410 Matrix33_GetTranspose(&rotA, &rotAt);
00411
00412 gePhysicsObject_GetInertiaTensor(podA, &iTensorA);
00413 gePhysicsObject_GetInertiaTensorInverse(podA, &iTensorInvA);
00414
00415 Matrix33_Multiply(&rotA, &iTensorA, &tmpMat);
00416 Matrix33_Multiply(&tmpMat, &rotAt, &itA);
00417
00418 Matrix33_Multiply(&rotA, &iTensorInvA, &tmpMat);
00419 Matrix33_Multiply(&tmpMat, &rotAt, &itiA);
00420
00421 gePhysicsObject_GetAngularVelocity(podA, &angularVelocityA, si);
00422
00423 Matrix33_MultiplyVec3d(&rotA, &angularVelocityA, &omegaA);
00424 Matrix33_MultiplyVec3d(&itA, &omegaA, &LA);
00425
00427
00428
00429 Matrix33_MultiplyScalar(gePhysicsObject_GetOneOverMass(podA), &gePhysicsSystemIdentityMatrix, &term11);
00430 Matrix33_Multiply(&rStarA, &itiA, &tmpMat);
00431 Matrix33_Multiply(&tmpMat, &rStarA, &term12);
00432 Matrix33_Subtract(&term11, &term12, &Mblock);
00433
00435
00436
00437 geVec3d_CrossProduct(&omegaA, &LA, &zeroForceAccelerationA);
00438 geVec3d_CrossProduct(&omegaA, &rA, &ptVelocityA);
00439 geVec3d_CrossProduct(&omegaA, &ptVelocityA, &alphaA);
00440 geVec3d_Add(&zeroForceAccelerationA, &alphaA, &ptAccelerationA);
00441
00443
00444
00445 Matrix33_MultiplyVec3d(&tmpMat, &ptAccelerationA, &beta);
00446
00447 gePhysicsObject_GetLinearVelocity(podA, &linearVelocityA, si);
00448 geVec3d_Add(&linearVelocityA, &ptVelocityA, &D1);
00449
00450 gePhysicsJoint_GetLocationAInWorldSpace(jntData, &jntLocAWS);
00451 gePhysicsJoint_GetLocationB(jntData, &jntLocB);
00452 geVec3d_Subtract(&jntLocAWS, &jntLocB, &D0);
00453
00455
00456
00457 geVec3d_Scale(&D1, 2.f / h, &D1);
00458 geVec3d_Scale(&D0, 1.f / hSquared, &D0);
00459 geVec3d_Add(&beta, &D1, &K);
00460 geVec3d_Add(&D0, &K, &K);
00461
00463
00464
00465 PS->linsys->KVector[iOffset] = -K.X;
00466 PS->linsys->KVector[iOffset + 1] = -K.Y;
00467 PS->linsys->KVector[iOffset + 2] = -K.Z;
00468
00469 for (j = 0; j < 3; j++)
00470 {
00471 for (k = 0; k < 3; k++)
00472 {
00473 PS->linsys->M[iOffset + j][iOffset + k] = Mblock.x[j][k];
00474 }
00475 }
00476
00477 iOffset += 3;
00478 break;
00479
00480 case JT_SPHERICAL:
00481
00483
00484
00486
00487
00488 podA = gePhysicsJoint_GetObject1(jntData);
00489
00490 assert(podA != NULL);
00491
00492 gePhysicsObject_GetLocation(podA, &offsetVecA, si);
00493 gePhysicsObject_GetXForm(podA, &xformA, si);
00494 gePhysicsJoint_GetLocationA(jntData, &jntLocA);
00495
00496 geXForm3d_Rotate(&xformA, &jntLocA, &rA);
00497 geVec3d_Add(&offsetVecA, &rA, &tmpVec);
00498 gePhysicsJoint_SetLocationAInWorldSpace(jntData, &tmpVec);
00499
00501
00502
00503 podB = gePhysicsJoint_GetObject2(jntData);
00504
00505 assert(podB != NULL);
00506
00507 gePhysicsObject_GetLocation(podB, &offsetVecB, si);
00508 gePhysicsObject_GetXForm(podB, &xformB, si);
00509 gePhysicsJoint_GetLocationB(jntData, &jntLocB);
00510
00511 geXForm3d_Rotate(&xformB, &jntLocB, &rB);
00512 geVec3d_Add(&offsetVecB, &rB, &tmpVec);
00513 gePhysicsJoint_SetLocationBInWorldSpace(jntData, &tmpVec);
00514
00516
00517
00519
00520
00521 Matrix33_MakeCrossProductMatrix33(&rA, &rStarA);
00522
00523 Matrix33_ExtractFromXForm3d(&xformA, &rotA);
00524 Matrix33_GetTranspose(&rotA, &rotAt);
00525
00526 gePhysicsObject_GetInertiaTensor(podA, &iTensorA);
00527 gePhysicsObject_GetInertiaTensorInverse(podA, &iTensorInvA);
00528
00529 Matrix33_Multiply(&rotA, &iTensorA, &tmpMat);
00530 Matrix33_Multiply(&tmpMat, &rotAt, &itA);
00531
00532 Matrix33_Multiply(&rotA, &iTensorInvA, &tmpMat);
00533 Matrix33_Multiply(&tmpMat, &rotAt, &itiA);
00534
00535 gePhysicsObject_GetAngularVelocity(podA, &angularVelocityA, si);
00536
00537 Matrix33_MultiplyVec3d(&rotA, &angularVelocityA, &omegaA);
00538 Matrix33_MultiplyVec3d(&itA, &omegaA, &LA);
00539
00541
00542
00543 Matrix33_MakeCrossProductMatrix33(&rB, &rStarB);
00544
00545 Matrix33_ExtractFromXForm3d(&xformB, &rotB);
00546 Matrix33_GetTranspose(&rotB, &rotBt);
00547
00548 gePhysicsObject_GetInertiaTensor(podB, &iTensorB);
00549 gePhysicsObject_GetInertiaTensorInverse(podB, &iTensorInvB);
00550
00551 Matrix33_Multiply(&rotB, &iTensorB, &tmpMat);
00552 Matrix33_Multiply(&tmpMat, &rotBt, &itB);
00553
00554 Matrix33_Multiply(&rotB, &iTensorInvB, &tmpMat);
00555 Matrix33_Multiply(&tmpMat, &rotBt, &itiB);
00556
00557 gePhysicsObject_GetAngularVelocity(podB, &angularVelocityB, si);
00558
00559 Matrix33_MultiplyVec3d(&rotB, &angularVelocityB, &omegaB);
00560 Matrix33_MultiplyVec3d(&itB, &omegaB, &LB);
00561
00563
00564
00565 Matrix33_MultiplyScalar(gePhysicsObject_GetOneOverMass(podA) +
00566 gePhysicsObject_GetOneOverMass(podB), &gePhysicsSystemIdentityMatrix, &term11);
00567
00568 Matrix33_Multiply(&rStarA, &itiA, &tmpMat);
00569 Matrix33_Multiply(&tmpMat, &rStarA, &term12);
00570
00571 Matrix33_Multiply(&rStarB, &itiB, &tmpMat);
00572 Matrix33_Multiply(&tmpMat, &rStarB, &term22);
00573
00574 Matrix33_Add(&term12, &term22, &term21);
00575
00576 Matrix33_Subtract(&term11, &term21, &Mblock);
00577
00579
00580
00581 geVec3d_CrossProduct(&omegaA, &LA, &zeroForceAccelerationA);
00582 geVec3d_CrossProduct(&omegaA, &rA, &ptVelocityA);
00583 geVec3d_CrossProduct(&omegaA, &ptVelocityA, &alphaA);
00584 geVec3d_Add(&zeroForceAccelerationA, &alphaA, &ptAccelerationA);
00585
00586 geVec3d_CrossProduct(&omegaB, &LB, &zeroForceAccelerationB);
00587 geVec3d_CrossProduct(&omegaB, &rB, &ptVelocityB);
00588 geVec3d_CrossProduct(&omegaB, &ptVelocityB, &alphaB);
00589 geVec3d_Add(&zeroForceAccelerationB, &alphaB, &ptAccelerationB);
00590
00591 Matrix33_MultiplyVec3d(&itiA, &ptAccelerationA, &tmpVec);
00592 Matrix33_MultiplyVec3d(&rStarA, &tmpVec, &accA);
00593
00594 Matrix33_MultiplyVec3d(&itiB, &ptAccelerationB, &tmpVec);
00595 Matrix33_MultiplyVec3d(&rStarB, &tmpVec, &accB);
00596
00597 geVec3d_Subtract(&accA, &accB, &beta);
00598
00599 gePhysicsObject_GetLinearVelocity(podA, &linearVelocityA, si);
00600 geVec3d_Add(&linearVelocityA, &ptVelocityA, &velA);
00601 gePhysicsObject_GetLinearVelocity(podB, &linearVelocityB, si);
00602 geVec3d_Add(&linearVelocityB, &ptVelocityB, &velB);
00603
00604 geVec3d_Subtract(&velA, &velB, &D1);
00605
00606 gePhysicsJoint_GetLocationAInWorldSpace(jntData, &jntLocAWS);
00607 gePhysicsJoint_GetLocationBInWorldSpace(jntData, &jntLocBWS);
00608
00609 geVec3d_Subtract(&jntLocAWS, &jntLocBWS, &D0);
00610
00612
00613
00614 geVec3d_Scale(&D1, 2.f / h, &D1);
00615 geVec3d_Scale(&D0, 1.f / hSquared, &D0);
00616 geVec3d_Add(&beta, &D1, &K);
00617 geVec3d_Add(&D0, &K, &K);
00618
00620
00621
00622 PS->linsys->KVector[iOffset] = -K.X;
00623 PS->linsys->KVector[iOffset + 1] = -K.Y;
00624 PS->linsys->KVector[iOffset + 2] = -K.Z;
00625
00626 for (j = 0; j < 3; j++)
00627 {
00628 for (k = 0; k < 3; k++)
00629 {
00630 PS->linsys->M[iOffset + j][iOffset + k] = Mblock.x[j][k];
00631 }
00632 }
00633
00634 iOffset += 3;
00635 break;
00636
00637 default:
00638 assert(!"Illegal joint type");
00639 break;
00640 }
00641 }
00642
00643 if (!gePhysicsSystem_SolveForConstraintForces(PS))
00644 {
00645 return GE_FALSE;
00646 }
00647
00648 for (i = 0, iOffset = 0; i < PS->PhysicsJointCount; i++)
00649 {
00650 jntData = PS->Joints[i];
00651
00653
00654
00655 type = gePhysicsJoint_GetType(jntData);
00656
00657 switch(type)
00658 {
00659 case JT_WORLD:
00660
00661 podA = gePhysicsJoint_GetObject1(jntData);
00662 assert(podA != NULL);
00663
00664 gePhysicsObject_GetXForm(podA, &xformA, si);
00665 gePhysicsJoint_GetLocationA(jntData, &jntLocA);
00666 geXForm3d_Rotate(&xformA, &jntLocA, &rA);
00667
00668 constraintForce.X = PS->linsys->X[iOffset];
00669 constraintForce.Y = PS->linsys->X[iOffset + 1];
00670 constraintForce.Z = PS->linsys->X[iOffset + 2];
00671
00672 gePhysicsObject_ApplyGlobalFrameForce(podA, &constraintForce, &rA, GE_FALSE, si);
00673
00674 iOffset += 3;
00675
00676 break;
00677
00678 case JT_SPHERICAL:
00679
00680 podA = gePhysicsJoint_GetObject1(jntData);
00681 assert(podA != NULL);
00682
00683 gePhysicsObject_GetXForm(podA, &xformA, si);
00684 gePhysicsJoint_GetLocationA(jntData, &jntLocA);
00685 geXForm3d_Rotate(&xformA, &jntLocA, &rA);
00686
00687 constraintForce.X = PS->linsys->X[iOffset];
00688 constraintForce.Y = PS->linsys->X[iOffset + 1];
00689 constraintForce.Z = PS->linsys->X[iOffset + 2];
00690
00691 gePhysicsObject_ApplyGlobalFrameForce(podA, &constraintForce, &rA, GE_FALSE, si);
00692
00694
00695
00696 podB = gePhysicsJoint_GetObject2(jntData);
00697 assert(podB != NULL);
00698
00699 gePhysicsObject_GetXForm(podB, &xformB, si);
00700 gePhysicsJoint_GetLocationB(jntData, &jntLocB);
00701 geXForm3d_Rotate(&xformB, &jntLocB, &rB);
00702
00703 constraintForce.X = -constraintForce.X;
00704 constraintForce.Y = -constraintForce.Y;
00705 constraintForce.Z = -constraintForce.Z;
00706
00707 gePhysicsObject_ApplyGlobalFrameForce(podB, &constraintForce, &rB, GE_FALSE, si);
00708
00709 iOffset += 3;
00710
00711 break;
00712
00713 default:
00714 break;
00715 }
00716 }
00717
00718 return GE_TRUE;
00719 }
00720
00721 static int imin(int a, int b)
00722 {
00723 return a < b ? a : b;
00724 }
00725
00726 static geBoolean gePhysicsSystem_SolveForConstraintForces(gePhysicsSystem* PS)
00727 {
00728 int i, j, k, n;
00729 int ixend1, ixend2;
00730 geFloat num;
00731 geFloat** M;
00732 geFloat* b;
00733 geFloat* x;
00734
00735 assert(PS != NULL);
00736
00737 b = PS->linsys->KVector;
00738 x = PS->linsys->X;
00739 M = PS->linsys->M;
00740
00741 assert(b != NULL);
00742 assert(x != NULL);
00743 assert(M != NULL);
00744
00745 n = PS->sumOfConstraintDimensions;
00746
00747 for (i = 0; i < n; i++)
00748 {
00749 assert(M[i] != NULL);
00750
00751 if ((geFloat)fabs(M[i][i]) < (geFloat)(1e-5))
00752 {
00753 return GE_FALSE;
00754 }
00755
00756 num = 1 / M[i][i];
00757
00758 for (j = i; j < n; j++)
00759 M[i][j] *= num;
00760
00761 b[i] *= num;
00762
00763
00764
00765 ixend1 = imin(n, i + 3);
00766
00767 for (j = i + 1; j < ixend1; j++)
00768 {
00769 num = M[j][i];
00770
00771 ixend2 = imin(n, i + 2);
00772
00773 for (k = i; k < ixend2; k++)
00774 {
00775 M[i][k] -= num * M[i][k];
00776 }
00777
00778 b[j] -= num * b[i];
00779 }
00780 }
00781
00783
00784
00785 for (i = n - 1; i >= 0; i--)
00786 {
00787 x[i] = b[i];
00788
00789 ixend1 = imin(n, i + 2);
00790
00791 for (j = i + 1; j < ixend1; j++)
00792 {
00793 x[i] -= M[i][j] * x[j];
00794 }
00795 }
00796
00797 return GE_TRUE;
00798 }
00799
00800 GENESISAPI int GENESISCC gePhysicsSystem_GetSourceConfigIndex(const gePhysicsSystem* pSys)
00801 {
00802 assert(pSys != NULL);
00803
00804 return pSys->sourceConfigIndex;
00805 }
00806
00807 GENESISAPI gePhysicsObject** GENESISCC gePhysicsSystem_GetPhysicsObjects(const gePhysicsSystem* pSys)
00808 {
00809 assert(pSys != NULL);
00810
00811 return pSys->Objects;
00812 }
00813
00814 GENESISAPI gePhysicsJoint** GENESISCC gePhysicsSystem_GetPhysicsJoints(const gePhysicsSystem* pSys)
00815 {
00816 assert(pSys != NULL);
00817
00818 return pSys->Joints;
00819 }
00820
00821 GENESISAPI int GENESISCC gePhysicsSystem_GetNumPhysicsObjects(const gePhysicsSystem* pSys)
00822 {
00823 assert(pSys != NULL);
00824
00825 return pSys->PhysicsObjectCount;
00826 }
00827
00828 GENESISAPI int GENESISCC gePhysicsSystem_GetNumPhysicsJoints(const gePhysicsSystem* pSys)
00829 {
00830 assert(pSys != NULL);
00831
00832 return pSys->PhysicsJointCount;
00833 }
00834
00835 GENESISAPI int GENESISCC gePhysicsSystem_GetSumOfConstraintDimensions(const gePhysicsSystem* pSys)
00836 {
00837 assert(pSys != NULL);
00838
00839 return pSys->sumOfConstraintDimensions;
00840 }
00841
