488 lines
14 KiB
C++
488 lines
14 KiB
C++
#include "SCENE.h"
|
|
static volatile u32 IKFREEZER = 1;
|
|
//#pragma optimize("",off)
|
|
void WORLD::DrawSmallSceneIK(SmallScene *SmSc)
|
|
{
|
|
MATRIX MatrixToSet,CP;
|
|
_3D_Object *pObj;
|
|
MatrixToSet.Identity();
|
|
CP = SmSc->CameraPosition;
|
|
CP.K *= -1.0f;
|
|
CP.Inverse(MatrixToSet);//*/
|
|
if (SmSc->IK == NULL) return;
|
|
ComputeSmallSceneHierarchie(SmSc);
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Father;
|
|
RS LineRS;
|
|
MATRIX WM;
|
|
Vector A,B;
|
|
pObj = SmSc->ObjectsPtrs[SmSc->IK->pObjPatchs[G].ObjectRefIndex];
|
|
Father = pObj->FatherIndex;
|
|
if(Father != -1)
|
|
{
|
|
GLOB_SetDefaultRS(&LineRS);
|
|
DI->Matrix(&MatrixToSet);
|
|
LineRS.NOZTEST = 1;
|
|
DI->SetRS(&LineRS,-1);
|
|
A = pObj->GlobalMatrix.T;
|
|
B = SmSc->ObjectsPtrs[Father]->GlobalMatrix.T;
|
|
DI->DrawLine(0xFFFFFF,&A,&B);
|
|
}
|
|
}
|
|
|
|
for ( int i = 0 ; i < SmSc->IK->ulNumberOfCollider; i++ )
|
|
{
|
|
u32 Father,Current;
|
|
RS LineRS;
|
|
MATRIX WM,WMi;
|
|
Vector A,B,CPiC;
|
|
pObj = SmSc->ObjectsPtrs[SmSc->IK->pColliders[i].ObjectRefIndex];
|
|
Father = pObj->FatherIndex;
|
|
WM.Identity();
|
|
WM.I &= SmSc->IK->pColliders[i].Radius;
|
|
WM.J &= SmSc->IK->pColliders[i].Radius;
|
|
WM.K &= SmSc->IK->pColliders[i].Radius;
|
|
WM.T = SmSc->IK->pColliders[i].Center;
|
|
WM *= pObj->GlobalMatrix;
|
|
WM.Inverse(WMi);
|
|
|
|
CPiC = WMi * CP.T;
|
|
|
|
WM *= MatrixToSet;
|
|
GLOB_SetDefaultRS(&LineRS);
|
|
DI->Matrix(&WM);
|
|
//LineRS.NOZTEST = 1;
|
|
DI->SetRS(&LineRS,-1);
|
|
#define DISC_SEG 16
|
|
#define DISC_SEG2 16
|
|
|
|
|
|
float X,Y,XN,YN,Alpha,Alpha2;
|
|
Alpha2 = 0;
|
|
u32 SegCounter2 = DISC_SEG2;
|
|
while (SegCounter2--)
|
|
{
|
|
u32 SegCounter = DISC_SEG;
|
|
Alpha2 += 3.1415927f / (float)(DISC_SEG2 + 1);
|
|
X = sinf(Alpha2);
|
|
Y = 0.0f;
|
|
Alpha = 0;
|
|
while (SegCounter--)
|
|
{
|
|
Alpha += 3.1415927f * 2.0f / (float)(DISC_SEG);
|
|
XN = cosf(Alpha) * sinf(Alpha2);
|
|
YN = sinf(Alpha) * sinf(Alpha2);
|
|
A = Vector(cosf(Alpha2),X,Y);
|
|
B = Vector(cosf(Alpha2),XN,YN);
|
|
if (CPiC * A > 0.0f)
|
|
DI->DrawLine(0x8080,&A,&B);
|
|
A = Vector(X,cosf(Alpha2),Y);
|
|
B = Vector(XN,cosf(Alpha2),YN);
|
|
if (CPiC * A > 0.0f)
|
|
DI->DrawLine(0x8080,&A,&B);
|
|
A = Vector(X,Y,cosf(Alpha2));
|
|
B = Vector(XN,YN,cosf(Alpha2));
|
|
if (CPiC * A > 0.0f)
|
|
DI->DrawLine(0x8080,&A,&B);
|
|
X = XN;
|
|
Y = YN;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void WORLD::ComputeLocalMatrixes_IK(SmallScene *SmSc)
|
|
{
|
|
for (int Opi = 0 ; Opi < SmSc->IK->ulNumberOfPatchs ; Opi++)
|
|
{
|
|
/* recompute GlobalMatrix IJK */
|
|
u32 Fi;
|
|
MATRIX mFi;
|
|
MATRIX mGi;
|
|
u32 G = SmSc->IK->pObjPatchs[Opi].ObjectRefIndex;
|
|
Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
|
|
if (Fi != -1)
|
|
{
|
|
Vector To,TIK;
|
|
MATRIX TMat,mFi;
|
|
MATRIX LocaLIK;
|
|
SmSc->ObjectsPtrs[Fi]->GlobalMatrix.Inverse(mFi);
|
|
LocaLIK = SmSc->ObjectsPtrs[G]->GlobalMatrix;
|
|
LocaLIK *= mFi;
|
|
To = SmSc->ObjectsPtrs[G]->OriginalLocalMatrix.T ;
|
|
TIK = LocaLIK.T;
|
|
/* Rotate LM */
|
|
Vector V1,V2,VCP;
|
|
Quat CP;
|
|
CP.ExtractFrom2Edges(&To,&TIK);
|
|
CP.TransforToMatrix(&TMat);
|
|
VCP = SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
|
|
TMat *= SmSc->ObjectsPtrs[G]->OriginalLocalMatrix;
|
|
TMat *= SmSc->ObjectsPtrs[Fi]->GlobalMatrix;
|
|
SmSc->ObjectsPtrs[G]->GlobalMatrix = TMat;
|
|
SmSc->ObjectsPtrs[G]->GlobalMatrix.T = VCP;
|
|
}
|
|
}
|
|
}
|
|
|
|
u32 IsNan(float F)
|
|
{
|
|
if ((*(u32 *)&F & 0x7f800000) == 0x7f800000)
|
|
return 1;
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
void WORLD::ComputeSmallSceneIK(SmallScene *SmSc,u32 Instance)
|
|
{
|
|
IK_ObjectPatchInstance *pCurrentHinstance;
|
|
_3D_Object **pObj;
|
|
|
|
if (Instance > 64) return;
|
|
if (SmSc->IK == NULL) return;
|
|
|
|
ComputeSmallSceneHierarchie(SmSc);
|
|
|
|
/* Verify and Set the number of Instance */
|
|
if (SmSc->IK->ulNumberOfInstance <= Instance)
|
|
{
|
|
IK_ObjectPatchInstance **ppPreviousPI;
|
|
ppPreviousPI = SmSc->IK->pPI;
|
|
SmSc->IK->pPI = (IK_ObjectPatchInstance **)CC_malloc(sizeof(IK_ObjectPatchInstance *) * (Instance + 1));
|
|
memset(SmSc->IK->pPI,0,sizeof(IK_ObjectPatchInstance *) * (Instance + 1));
|
|
if (ppPreviousPI)
|
|
{
|
|
for (int K = 0;K < SmSc->IK->ulNumberOfInstance ; K++)
|
|
SmSc->IK->pPI[K] = ppPreviousPI[K];
|
|
CC_free(ppPreviousPI);
|
|
}
|
|
SmSc->IK->ulNumberOfInstance = Instance + 1;
|
|
}
|
|
if (!SmSc->IK->pPI[Instance])
|
|
{
|
|
SmSc->IK->pPI[Instance] = (IK_ObjectPatchInstance *)CC_malloc(sizeof(IK_ObjectPatchInstance) * (SmSc->IK->ulNumberOfPatchs));
|
|
memset(SmSc->IK->pPI[Instance],0,sizeof(IK_ObjectPatchInstance) * (SmSc->IK->ulNumberOfPatchs));
|
|
pCurrentHinstance = SmSc->IK->pPI[Instance];
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi;
|
|
Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
pCurrentHinstance[G].ComputedMatrix = SmSc->ObjectsPtrs[Oi]->OriginalGlobalMatrix;
|
|
pCurrentHinstance[G].LastGlobalMatrix = SmSc->ObjectsPtrs[Oi]->OriginalGlobalMatrix;
|
|
pCurrentHinstance[G].vSpeed = Vector(0,0,0);
|
|
}
|
|
SmSc->IK->LastTimeCalled[Instance] = timeGetTime() - 20;
|
|
}
|
|
|
|
pCurrentHinstance = SmSc->IK->pPI[Instance];
|
|
|
|
//if (UpdateWithoutCompute) return;
|
|
/* Compute DT */
|
|
float GDT;
|
|
u32 LoopNum;
|
|
LoopNum = timeGetTime();
|
|
if (LoopNum - SmSc->IK->LastTimeCalled[Instance] > 200)
|
|
SmSc->IK->LastTimeCalled[Instance] = timeGetTime()-200;
|
|
#define IK_BIG_DT 16
|
|
|
|
|
|
if (SmSc->IK->LastTimeCalled[Instance] + IK_BIG_DT >= LoopNum)
|
|
{
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
if (pCurrentHinstance[G].ComputedMatrix.CheckNAN())
|
|
pCurrentHinstance[G].ComputedMatrix.Identity();
|
|
SmSc->ObjectsPtrs[Oi]->GlobalMatrix = pCurrentHinstance[G].ComputedMatrix;
|
|
}
|
|
for (int G = 0 ; G < SmSc->ulNumberOfObjects ; G++)
|
|
{
|
|
/* recompute localmatrixes */
|
|
u32 Fi;
|
|
MATRIX mFi;
|
|
MATRIX mGi;
|
|
Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
|
|
if (Fi != -1)
|
|
{
|
|
SmSc->ObjectsPtrs[Fi]->GlobalMatrix.Inverse(mFi);
|
|
SmSc->ObjectsPtrs[G]->LocalMatrix = SmSc->ObjectsPtrs[G]->GlobalMatrix;
|
|
SmSc->ObjectsPtrs[G]->LocalMatrix *= mFi;
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
|
|
/* RESET TEST in case of big JUMPS (movie cut or...), detect it and solve it */
|
|
/* BEGIN *********************************************************************/
|
|
if ((SmSc->IK->ulNumberOfInstance <= 1))
|
|
{
|
|
u32 MustFreeze = 0;
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
Quat A,B;
|
|
Vector D;
|
|
D = SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T - pCurrentHinstance[G].LastGlobalMatrix.T;
|
|
if (D.Norm() > 100.0f)
|
|
MustFreeze= 1;
|
|
else
|
|
{
|
|
A.ComputeFromMatrix(&SmSc->ObjectsPtrs[Oi]->GlobalMatrix);
|
|
B.ComputeFromMatrix(&pCurrentHinstance[G].LastGlobalMatrix);
|
|
if (((*(Vector *)&A) ^ (*(Vector *)&B)).Norm() > 0.03f)
|
|
{
|
|
MustFreeze= 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (MustFreeze)
|
|
// Then "FREEZE" IK.
|
|
{
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
|
|
MATRIX TMAT;
|
|
pCurrentHinstance[G].LastGlobalMatrix.Inverse(TMAT);
|
|
TMAT *= SmSc->ObjectsPtrs[Oi]->GlobalMatrix;
|
|
pCurrentHinstance[G].ComputedMatrix *= TMAT;
|
|
if (MustFreeze)
|
|
pCurrentHinstance[G].vSpeed = Vector(0,0,0);
|
|
SmSc->ObjectsPtrs[Oi]->NV = Vector(0,0,0);
|
|
pCurrentHinstance[G].LastGlobalMatrix = SmSc->ObjectsPtrs[Oi]->GlobalMatrix;
|
|
SmSc->ObjectsPtrs[Oi]->GlobalMatrix = pCurrentHinstance[G].ComputedMatrix;
|
|
}
|
|
for (int G = 0 ; G < SmSc->ulNumberOfObjects ; G++)
|
|
{
|
|
/* recompute localmatrixes */
|
|
u32 Fi;
|
|
MATRIX mFi;
|
|
MATRIX mGi;
|
|
Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
|
|
if (Fi != -1)
|
|
{
|
|
SmSc->ObjectsPtrs[Fi]->GlobalMatrix.Inverse(mFi);
|
|
SmSc->ObjectsPtrs[G]->LocalMatrix = SmSc->ObjectsPtrs[G]->GlobalMatrix;
|
|
SmSc->ObjectsPtrs[G]->LocalMatrix *= mFi;
|
|
}
|
|
}
|
|
MustFreeze = 0;
|
|
return;
|
|
}
|
|
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
pCurrentHinstance[G].LastGlobalMatrix = SmSc->ObjectsPtrs[Oi]->GlobalMatrix;
|
|
}
|
|
}
|
|
/* END *********************************************************************/
|
|
|
|
|
|
|
|
|
|
/* Compute mechanics */
|
|
|
|
while (SmSc->IK->LastTimeCalled[Instance] + IK_BIG_DT < LoopNum)
|
|
{
|
|
SmSc->IK->LastTimeCalled[Instance] += IK_BIG_DT;
|
|
GDT = 1.0f / 30.0f;
|
|
|
|
/* Reset forces */
|
|
pObj = SmSc->ObjectsPtrs;
|
|
for ( int i = 0 ; i < SmSc->ulNumberOfObjects ; i++ , pObj++)
|
|
(*pObj)->NV = Vector(0,0,0);
|
|
|
|
/* Compute forces */
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
SmSc->ObjectsPtrs[Oi]->GlobalMatrix = pCurrentHinstance[G].ComputedMatrix;
|
|
|
|
//Gravity forces
|
|
SmSc->ObjectsPtrs[Oi]->NV += SmSc->IK->pObjPatchs[G].vWeight * GDT;
|
|
// Torque forces
|
|
/* u32 Fi;
|
|
Fi = SmSc->ObjectsPtrs[Oi]->FatherIndex;
|
|
if (Fi != -1)
|
|
{
|
|
Vector To,Tk;
|
|
float PS,nTK;
|
|
To = SmSc->ObjectsPtrs[Fi]->GlobalMatrix * (SmSc->ObjectsPtrs[Oi]->OriginalLocalMatrix.T);
|
|
To = To - SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T;
|
|
SmSc->ObjectsPtrs[Oi]->NV += To * (GDT * SmSc->IK->pObjPatchs[G].fSpringStrength);
|
|
SmSc->ObjectsPtrs[Fi]->NV -= To * (GDT * SmSc->IK->pObjPatchs[G].fSpringStrength);
|
|
}*/
|
|
}
|
|
// Apply forces
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
pCurrentHinstance[G].vSpeed += SmSc->ObjectsPtrs[Oi]->NV * GDT;
|
|
SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T += pCurrentHinstance[G].vSpeed * GDT;
|
|
}//*/
|
|
|
|
// Compute matrix colliders
|
|
for ( int i = 0 ; i < SmSc->IK->ulNumberOfCollider; i++ )
|
|
{
|
|
pObj = &SmSc->ObjectsPtrs[SmSc->IK->pColliders[i].ObjectRefIndex];
|
|
SmSc->IK->pColliders[i].TransMatrix.Identity();
|
|
SmSc->IK->pColliders[i].TransMatrix.I &= SmSc->IK->pColliders[i].Radius;
|
|
SmSc->IK->pColliders[i].TransMatrix.J &= SmSc->IK->pColliders[i].Radius;
|
|
SmSc->IK->pColliders[i].TransMatrix.K &= SmSc->IK->pColliders[i].Radius;
|
|
SmSc->IK->pColliders[i].TransMatrix.T = SmSc->IK->pColliders[i].Center;
|
|
SmSc->IK->pColliders[i].TransMatrix *= (*pObj)->GlobalMatrix;
|
|
SmSc->IK->pColliders[i].TransMatrix.Inverse(SmSc->IK->pColliders[i].InvMatrix);
|
|
} //*/
|
|
|
|
/* Compute IK */
|
|
for (u32 IKLoop = 0; IKLoop < 12 ; IKLoop++)
|
|
{
|
|
ComputeLocalMatrixes_IK(SmSc);
|
|
pObj = SmSc->ObjectsPtrs;
|
|
for ( int i = 0 ; i < SmSc->ulNumberOfObjects ; i++ , pObj++)
|
|
(*pObj)->NV = Vector(0,0,0);
|
|
pObj = SmSc->ObjectsPtrs;
|
|
for ( int i = 0 ; i < SmSc->ulNumberOfObjects ; i++ , pObj++)
|
|
{
|
|
float N;
|
|
N = (*pObj)->GlobalMatrix.I.Norm();
|
|
N = (*pObj)->GlobalMatrix.J.Norm();
|
|
N = (*pObj)->GlobalMatrix.K.Norm();
|
|
}
|
|
for (int G0 = 0 ; G0 < SmSc->IK->ulNumberOfPatchs ; G0++)
|
|
{
|
|
u32 G = SmSc->IK->pObjPatchs[G0].ObjectRefIndex;
|
|
float DistanceToF,DistanceToFIK;
|
|
Vector NormalDTFK;
|
|
u32 Fi;
|
|
Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
|
|
if (Fi != -1)
|
|
{
|
|
/* Max angle constraint */
|
|
if ((IKLoop > 3)&&(SmSc->IK->pObjPatchs[G0].MaxAngle < 1.6f))
|
|
{
|
|
Vector To,Tk;
|
|
Vector ToN,TkN,okCP;
|
|
float PS,nTK;
|
|
To = SmSc->ObjectsPtrs[Fi]->GlobalMatrix * SmSc->ObjectsPtrs[G]->OriginalLocalMatrix.T;
|
|
Tk = SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
|
|
To -= SmSc->ObjectsPtrs[Fi]->GlobalMatrix.T;
|
|
Tk -= SmSc->ObjectsPtrs[Fi]->GlobalMatrix.T;
|
|
ToN = To;
|
|
TkN = Tk;
|
|
ToN.Normalize();
|
|
TkN.Normalize();
|
|
TkN += ToN;
|
|
TkN.Normalize();
|
|
okCP = ToN ^ TkN;
|
|
if (okCP.Norm() > SmSc->IK->pObjPatchs[G0].MaxAngle)
|
|
{
|
|
Quat CP,QVQ;
|
|
Vector VQ;
|
|
MATRIX Torque;
|
|
MATRIX TorqueMax;
|
|
|
|
CP.ExtractFrom2Edges(&Tk,&To);
|
|
VQ = *(Vector *)&CP;
|
|
VQ.Normalize();
|
|
VQ *= -SmSc->IK->pObjPatchs[G0].MaxAngle ;
|
|
QVQ = Quat(VQ);
|
|
CP= CP * QVQ;
|
|
CP.NormalizeW();
|
|
CP.TransforToMatrix(&Torque);
|
|
VQ = Torque * Tk;
|
|
Tk = VQ - Tk;
|
|
if (!Tk.CheckNAN())
|
|
{
|
|
SmSc->ObjectsPtrs[G]->GlobalMatrix.T += Tk*0.5f;
|
|
SmSc->ObjectsPtrs[Fi]->GlobalMatrix.T -= Tk*0.5f;
|
|
}
|
|
}
|
|
}
|
|
//Lenght Constraint
|
|
{
|
|
/* Bone lenght constraint */
|
|
DistanceToF = (SmSc->ObjectsPtrs[G]->OriginalGlobalMatrix.T - SmSc->ObjectsPtrs[Fi]->OriginalGlobalMatrix.T).Norm();
|
|
NormalDTFK = SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
|
|
NormalDTFK -= SmSc->ObjectsPtrs[Fi]->GlobalMatrix.T;
|
|
DistanceToFIK = NormalDTFK.Norm();
|
|
NormalDTFK.Normalize();
|
|
DistanceToFIK -= DistanceToF;
|
|
DistanceToFIK /= 2.0f;
|
|
NormalDTFK *= DistanceToFIK;
|
|
if (!NormalDTFK.CheckNAN())
|
|
{
|
|
SmSc->ObjectsPtrs[G]->NV -= NormalDTFK;
|
|
SmSc->ObjectsPtrs[Fi]->NV += NormalDTFK;
|
|
}
|
|
}
|
|
// Colliders constraint
|
|
if (SmSc->IK->ulNumberOfCollider)
|
|
{
|
|
|
|
IK_Collider *pCol,*pColLast;
|
|
pCol = SmSc->IK->pColliders;
|
|
pColLast = pCol + SmSc->IK->ulNumberOfCollider;
|
|
while (pCol < pColLast)
|
|
{
|
|
Vector Into;
|
|
Into = pCol->InvMatrix * SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
|
|
if (Into.SqrNorm() < 1.0f)
|
|
{
|
|
Into.Normalize();
|
|
Into = pCol->TransMatrix * Into;
|
|
Into -= SmSc->ObjectsPtrs[G]->GlobalMatrix.T;
|
|
if (!Into.CheckNAN())
|
|
SmSc->ObjectsPtrs[G]->NV += Into;
|
|
};
|
|
pCol++;
|
|
}
|
|
|
|
}//*/
|
|
}
|
|
}
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T += SmSc->ObjectsPtrs[Oi]->NV;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
for (int G = 0 ; G < SmSc->ulNumberOfObjects ; G++)
|
|
{
|
|
/* recompute localmatrixes */
|
|
u32 Fi;
|
|
MATRIX mFi;
|
|
MATRIX mGi;
|
|
Fi = SmSc->ObjectsPtrs[G]->FatherIndex;
|
|
if (Fi != -1)
|
|
{
|
|
SmSc->ObjectsPtrs[Fi]->GlobalMatrix.Inverse(mFi);
|
|
SmSc->ObjectsPtrs[G]->LocalMatrix = SmSc->ObjectsPtrs[G]->GlobalMatrix;
|
|
SmSc->ObjectsPtrs[G]->LocalMatrix *= mFi;
|
|
}
|
|
}
|
|
for (int G = 0 ; G < SmSc->IK->ulNumberOfPatchs ; G++)
|
|
{
|
|
u32 Oi = SmSc->IK->pObjPatchs[G].ObjectRefIndex;
|
|
pCurrentHinstance[G].vSpeed = SmSc->ObjectsPtrs[Oi]->GlobalMatrix.T - pCurrentHinstance[G].ComputedMatrix.T;
|
|
pCurrentHinstance[G].vSpeed *= 1.0f / GDT;
|
|
pCurrentHinstance[G].vSpeed *= 1.0f - SmSc->IK->pObjPatchs[G].VelocityAbsorbtion;
|
|
if (pCurrentHinstance[G].vSpeed.CheckNAN() || pCurrentHinstance[G].vSpeed.SqrNorm()> 10000.0)
|
|
pCurrentHinstance[G].vSpeed = Vector(0,0,0);
|
|
|
|
pCurrentHinstance[G].ComputedMatrix = SmSc->ObjectsPtrs[Oi]->GlobalMatrix;
|
|
// if (pCurrentHinstance[G].ComputedMatrix.CheckNAN()) pCurrentHinstance[G].ComputedMatrix.Identity();
|
|
}
|
|
}
|
|
|
|
}
|
|
|