1238 lines
54 KiB
C++
1238 lines
54 KiB
C++
//////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Rigid Body
|
|
//
|
|
// File: rigidbody.cpp
|
|
// Description : RigidBody class implementation
|
|
//
|
|
// History:
|
|
// -:Created by Anton Knyazev
|
|
//
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
#include "stdafx.h"
|
|
|
|
#include "rigidbody.h"
|
|
|
|
RigidBody::RigidBody()
|
|
{
|
|
P.zero(); L.zero();
|
|
v.zero(); w.zero();
|
|
q.SetIdentity();
|
|
pos.zero();
|
|
Ibody.zero();
|
|
bProcessed = 0; M=Minv=V = 0;
|
|
Iinv.SetZero();
|
|
Ibody.zero();
|
|
Ibody_inv.zero();
|
|
qfb.SetIdentity();
|
|
offsfb.zero();
|
|
Fcollision.zero(); Tcollision.zero();
|
|
pOwner = this;
|
|
Eunproj = 0;
|
|
softness[0] = 0.00015f;
|
|
softness[1] = 0.001f;
|
|
}
|
|
|
|
void RigidBody::Create(const vectorf ¢er,const vectorf &Ibody0,const quaternionf &q0, float volume,float mass,
|
|
const quaternionf &qframe,const vectorf &posframe)
|
|
{
|
|
float density=mass/volume;
|
|
|
|
V = volume;M = mass; q = q0; pos = center;
|
|
if (M>0) {
|
|
(Ibody_inv = Ibody = Ibody0*density).invert();
|
|
Minv = 1.0f/M;
|
|
} else {
|
|
Ibody_inv = Ibody.zero();
|
|
Minv = 0;
|
|
}
|
|
qfb = !q*qframe;
|
|
offsfb = (pos-posframe)*qframe;
|
|
|
|
UpdateState();
|
|
}
|
|
|
|
void RigidBody::Add(const vectorf ¢er,const vectorf Ibodyop,const quaternionf &qop, float volume,float mass)
|
|
{
|
|
if (mass==0.0f) return;
|
|
float density=mass/volume; int i;
|
|
matrix3x3RMf Rop,Ibodyop_mtx,Iop,Rbody2newbody,Ibody_mtx;
|
|
Ibodyop_mtx.SetIdentity(); for(i=0;i<3;i++) Ibodyop_mtx(i,i)=Ibodyop[i]*density;
|
|
|
|
//(!q*qop).getmatrix(Rop); //Q2M_IVO
|
|
Rop = matrix3x3f(!q*qop);
|
|
Iop = Rop*Ibodyop_mtx*Rop.T();
|
|
|
|
vectorf posnew = (pos*M+center*mass)/(M+mass);
|
|
Ibody_mtx = Ibody;
|
|
OffsetInertiaTensor(Ibody_mtx,(posnew-pos)*q,M);
|
|
OffsetInertiaTensor(Iop,(posnew-center)*q,mass);
|
|
M+=mass; V+=volume; Ibody_mtx+=Iop; Minv=1.0f/M;
|
|
|
|
quaternionf qframe = q*qfb;
|
|
offsfb += (posnew-pos)*qframe;
|
|
|
|
matrixf eigenBasis(3,3,0,Rbody2newbody.GetData());
|
|
matrixf(3,3,mtx_symmetric,Ibody_mtx.GetData()).jacobi_transformation(eigenBasis,&Ibody.x);
|
|
(Ibody_inv = Ibody).invert();
|
|
quaternion qb2nb = (quaternion)Rbody2newbody;
|
|
q *= !qb2nb; qfb = qb2nb*qfb; pos = posnew;
|
|
|
|
UpdateState();
|
|
}
|
|
|
|
void RigidBody::zero()
|
|
{
|
|
M = Minv = 0;
|
|
Ibody.zero(); Ibody_inv.zero();
|
|
}
|
|
|
|
void RigidBody::UpdateState()
|
|
{
|
|
//q.getmatrix(R); //Q2M_IVO
|
|
matrix3x3f R = matrix3x3f(q);
|
|
Iinv = R*Ibody_inv*R.T();
|
|
if (Minv>0) {
|
|
v = P*Minv;
|
|
w = Iinv*L;
|
|
}
|
|
}
|
|
|
|
void RigidBody::Step(float dt)
|
|
{
|
|
UpdateState();
|
|
|
|
pos += v*dt;
|
|
if (w.len2()*sqr(dt)<sqr(0.003f))
|
|
q += quaternionf(0,w*0.5f)*q*dt;
|
|
else {
|
|
float wlen = w.len();
|
|
//q = quaternionf(wlen*dt,w/wlen)*q;
|
|
q = GetRotationAA(wlen*dt,w/wlen)*q;
|
|
}
|
|
|
|
q.Normalize();
|
|
//q.getmatrix(R); //Q2M_IVO
|
|
matrix3x3f R = matrix3x3f(q);
|
|
Iinv = R*Ibody_inv*R.T();
|
|
w = Iinv*L;
|
|
}
|
|
|
|
void RigidBody::GetContactMatrix(const vectorf &r, matrix3x3f &K)
|
|
{
|
|
matrix3x3f rmtx,rmtx1;
|
|
//temporary change: crossproduct_matrix() is new a global function
|
|
((crossproduct_matrix(r,rmtx))*=Iinv) *= crossproduct_matrix(r,rmtx1);
|
|
K -= rmtx;
|
|
K(0,0)+=Minv; K(1,1)+=Minv; K(2,2)+=Minv;
|
|
}
|
|
|
|
void RigidBody::AddImpulseAtContact(entity_contact *pcontact, int iop, const vectorf &dP)
|
|
{
|
|
P += dP; L += pcontact->pt[iop]-pos^dP;
|
|
v = P*Minv; w = Iinv*L;
|
|
}
|
|
|
|
vectorf RigidBody::GetVelocityAtContact(entity_contact *pcontact, int iop)
|
|
{
|
|
return v + (w^pcontact->pt[iop]-pos);
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////
|
|
///////////////////// Global Multibody Solver //////////////////////////
|
|
////////////////////////////////////////////////////////////////////////
|
|
|
|
struct contact_helper {
|
|
vectorf r0,r1;
|
|
matrix3x3f K;
|
|
vectorf n;
|
|
vectorf vreq;
|
|
float Pspare;
|
|
float friction;
|
|
int flags;
|
|
int iBody[2];
|
|
int iCount;
|
|
int iCountDst;
|
|
};
|
|
struct body_helper {
|
|
vectorf v,w;
|
|
float Minv,M;
|
|
matrix3x3f Iinv;
|
|
vectorf L;
|
|
};
|
|
struct contact_sandwich {
|
|
int iMiddle;
|
|
int iBread[2];
|
|
contact_sandwich *next;
|
|
int bProcessed;
|
|
};
|
|
struct buddy_info {
|
|
int iBody;
|
|
vectorf vreq;
|
|
int flags;
|
|
buddy_info *next;
|
|
};
|
|
struct follower_thunk {
|
|
int iBody;
|
|
follower_thunk *next;
|
|
};
|
|
struct body_info {
|
|
buddy_info *pbuddy;
|
|
contact_sandwich *psandwich;
|
|
follower_thunk *pfollower;
|
|
float Minv;
|
|
int iLevel;
|
|
int idUpdate;
|
|
int idx;
|
|
vectorf v_unproj,w_unproj;
|
|
vectorf Fcollision,Tcollision;
|
|
};
|
|
|
|
int g_nContacts,g_nBodies;
|
|
static entity_contact *g_pContacts[MAX_CONTACTS];
|
|
static RigidBody *g_pBodies[MAX_CONTACTS];
|
|
static contact_helper g_Contacts[MAX_CONTACTS];
|
|
static body_helper g_Bodies[MAX_CONTACTS];
|
|
static body_info g_infos[MAX_CONTACTS];
|
|
static contact_sandwich g_sandwichbuf[MAX_CONTACTS];
|
|
static buddy_info g_buddybuf[MAX_CONTACTS];
|
|
static follower_thunk g_followersbuf[MAX_CONTACTS];
|
|
static int g_nFollowers,g_idLastUpdate,g_nUnprojLoops;
|
|
static char g_SolverBuf[262144];
|
|
static int g_iSolverBufPos;
|
|
bool g_bUsePreCG = true;
|
|
int __solver_step=0;
|
|
|
|
|
|
bool should_swap(entity_contact **pContacts,int i1,int i2) { // sub-sorts contacts by pbody[1]
|
|
return pContacts[i1]->pbody[1]->bProcessed < pContacts[i2]->pbody[1]->bProcessed;
|
|
}
|
|
bool should_swap(RigidBody **pBodies, int i1,int i2) { // sorts bodies by level
|
|
return g_infos[pBodies[i1]->bProcessed].iLevel < g_infos[pBodies[i2]->bProcessed].iLevel;
|
|
}
|
|
struct entity_contact_unproj : entity_contact {};
|
|
bool should_swap(entity_contact_unproj **pContacts,int i1,int i2) { // sorts contacts by newly sorted bodies
|
|
int iop1 = isneg(g_infos[pContacts[i1]->pbody[0]->bProcessed].iLevel-g_infos[pContacts[i1]->pbody[1]->bProcessed].iLevel);
|
|
int iop2 = isneg(g_infos[pContacts[i2]->pbody[0]->bProcessed].iLevel-g_infos[pContacts[i2]->pbody[1]->bProcessed].iLevel);
|
|
return g_infos[pContacts[i1]->pbody[iop1]->bProcessed].idx < g_infos[pContacts[i2]->pbody[iop2]->bProcessed].idx;
|
|
}
|
|
|
|
template<class dtype> static void qsort(dtype *pArray,int left,int right) {
|
|
if (left>=right) return;
|
|
int i,last;
|
|
swap(pArray, left,left+right>>1);
|
|
for(last=left,i=left+1; i<=right; i++)
|
|
if (should_swap(pArray,i,left))
|
|
swap(pArray, ++last, i);
|
|
swap(pArray, left,last);
|
|
|
|
qsort(pArray, left,last-1);
|
|
qsort(pArray, last+1,right);
|
|
}
|
|
#define bidx0(i) (g_pContacts[i]->pbody[0]->bProcessed)
|
|
#define bidx1(i) (g_pContacts[i]->pbody[1]->bProcessed)
|
|
|
|
void update_followers(int iBody,int idUpdate) {
|
|
if (g_infos[iBody].idUpdate==idUpdate) {
|
|
g_nUnprojLoops++; return;
|
|
}
|
|
g_infos[iBody].idUpdate = idUpdate;
|
|
for(follower_thunk *pfollower=g_infos[iBody].pfollower; pfollower; pfollower=pfollower->next)
|
|
if (g_infos[pfollower->iBody].iLevel<=g_infos[iBody].iLevel) {
|
|
g_infos[pfollower->iBody].iLevel = g_infos[iBody].iLevel+1;
|
|
update_followers(pfollower->iBody,idUpdate);
|
|
}
|
|
}
|
|
|
|
// trace_unproj_route recursive func: have 2 bodies (1 inside, 1 outside), find all possible 2nd outsides and call recurecively for each of them
|
|
// (maintain a list of "followers" for each such body); assign level to each processed body
|
|
void trace_unproj_route(int iMiddle,int iBread);
|
|
|
|
void add_route_follower(int iBody,int iFollower) {
|
|
follower_thunk* pfollower;
|
|
for(pfollower=g_infos[iBody].pfollower; pfollower && pfollower->iBody!=iFollower; pfollower=pfollower->next);
|
|
if (!pfollower && g_nFollowers<MAX_CONTACTS) {
|
|
g_followersbuf[g_nFollowers].iBody = iFollower;
|
|
g_followersbuf[g_nFollowers].next = g_infos[iBody].pfollower;
|
|
g_infos[iBody].pfollower = g_followersbuf+g_nFollowers++;
|
|
trace_unproj_route(iFollower, iBody);
|
|
}
|
|
}
|
|
|
|
void update_level(int iBody, int iNewLevel) {
|
|
if ((unsigned int)g_infos[iBody].iLevel>=(unsigned int)iNewLevel)
|
|
g_infos[iBody].iLevel = max(g_infos[iBody].iLevel, iNewLevel); // -1 or >=new level
|
|
else {
|
|
g_infos[iBody].iLevel = iNewLevel;
|
|
update_followers(iBody,++g_idLastUpdate);
|
|
}
|
|
}
|
|
|
|
void trace_unproj_route(int iMiddle,int iBread) {
|
|
int iop;
|
|
for(contact_sandwich *psandwich=g_infos[iMiddle].psandwich; psandwich; psandwich=psandwich->next)
|
|
if (iszero(psandwich->iMiddle-iMiddle) & ((iop=iszero(psandwich->iBread[0]-iBread)) | iszero(psandwich->iBread[1]-iBread))) {
|
|
psandwich->bProcessed = 1;
|
|
update_level(psandwich->iBread[iop], g_infos[iMiddle].iLevel+1);
|
|
add_route_follower(iMiddle,psandwich->iBread[iop]);
|
|
}
|
|
}
|
|
|
|
real ComputeRc(RigidBody *body0, entity_contact **pContacts, int nAngContacts,int nContacts) // for MINRES unprojection solver
|
|
{
|
|
int i;
|
|
real rT_x_rc=0;
|
|
vectorf dP,r0;
|
|
|
|
body0->Fcollision.zero(); body0->Tcollision.zero();
|
|
for(i=0;i<nAngContacts;i++) // angular contacts
|
|
body0->Tcollision += (pContacts[i]->dP*pContacts[i]->r.x)*pContacts[i]->Kinv(0,0);
|
|
for(;i<nContacts;i++) { // positional contacts
|
|
r0 = pContacts[i]->pt[pContacts[i]->flags>>contact_bidx_log2&1]-body0->pos;
|
|
dP = (pContacts[i]->dP*pContacts[i]->r.x)*pContacts[i]->Kinv(0,0);
|
|
body0->Fcollision += dP; body0->Tcollision += r0^dP;
|
|
}
|
|
for(i=0;i<nAngContacts;i++) // angular contacts
|
|
pContacts[i]->vrel = body0->Iinv*body0->Tcollision;
|
|
for(;i<nContacts;i++) { // positional contacts
|
|
r0 = pContacts[i]->pt[pContacts[i]->flags>>contact_bidx_log2&1]-body0->pos;
|
|
pContacts[i]->vrel = body0->Fcollision*body0->Minv + (body0->Iinv*body0->Tcollision^r0);
|
|
}
|
|
for(i=0;i<nContacts;i++)
|
|
rT_x_rc += (pContacts[i]->dP*pContacts[i]->vrel)*pContacts[i]->r.x*pContacts[i]->Kinv(0,0);
|
|
|
|
return rT_x_rc;
|
|
}
|
|
|
|
|
|
void InitContactSolver(float time_interval)
|
|
{
|
|
g_nContacts = g_nBodies = 0;
|
|
g_iSolverBufPos = 0;
|
|
g_bUsePreCG = true;
|
|
}
|
|
|
|
char *AllocSolverTmpBuf(int size)
|
|
{
|
|
if (g_iSolverBufPos+size<sizeof(g_SolverBuf)) {
|
|
g_iSolverBufPos += size;
|
|
return g_SolverBuf+g_iSolverBufPos-size;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void RegisterContact(entity_contact *pcontact)
|
|
{
|
|
if (!pcontact->pbody[0]->pOwner->OnRegisterContact(pcontact,0) || !pcontact->pbody[1]->pOwner->OnRegisterContact(pcontact,1))
|
|
return;
|
|
if (!(pcontact->flags & contact_maintain_count))
|
|
pcontact->pBounceCount = &pcontact->iCount;
|
|
g_pContacts[g_nContacts++] = pcontact;
|
|
g_nContacts = min(g_nContacts,(int)(sizeof(g_pContacts)/sizeof(g_pContacts[0]))-1);
|
|
}
|
|
|
|
void InvokeContactSolver(float time_interval, SolverSettings *pss)
|
|
{
|
|
FUNCTION_PROFILER( GetISystem(),PROFILE_PHYSICS );
|
|
|
|
if (g_nContacts==0) return;
|
|
int i,j,iop,nBodies,istart,iend,istep,iter,bBounced,nBounces,bContactBounced,nConstraints,nMaxIters;
|
|
RigidBody *body0,*body1;
|
|
body_helper *hbody0,*hbody1;
|
|
vectorf r0,r1,n,dp,dP,Kdp;
|
|
float t,vrel,Ebefore,Eafter,dPn,dPtang,rtime_interval=1/time_interval;
|
|
float e = pss->accuracyMC;
|
|
__solver_step++;
|
|
|
|
for(i=nBodies=nConstraints=0; i<g_nContacts; i++) {
|
|
for(iop=0;iop<2;iop++) {
|
|
if (!g_pContacts[i]->pbody[iop]->bProcessed) {
|
|
g_pBodies[nBodies++] = g_pContacts[i]->pbody[iop];
|
|
g_pContacts[i]->pbody[iop]->bProcessed = nBodies;
|
|
}
|
|
g_Contacts[i].iBody[iop] = g_pContacts[i]->pbody[iop]->bProcessed-1;
|
|
}
|
|
if (!(g_pContacts[i]->flags & contact_wheel))
|
|
g_pContacts[i]->Pspare = 0;
|
|
if (!(g_pContacts[i]->flags & contact_use_C))
|
|
g_pContacts[i]->C.SetIdentity();
|
|
nConstraints -= -(g_pContacts[i]->flags & contact_constraint)>>31;
|
|
g_pContacts[i]->P.zero();
|
|
g_pContacts[i]->iCount = 0;
|
|
g_pContacts[i]->bProcessed = i;
|
|
|
|
if (g_pContacts[i]->flags & contact_constraint_3dof)
|
|
(g_pContacts[i]->Kinv=g_pContacts[i]->K).Invert();
|
|
else if (g_pContacts[i]->flags & contact_constraint_1dof) {
|
|
vectorf axes[2]; int j,k; float mtx[2][2]; matrix3x3f mtx1;
|
|
axes[0] = g_pContacts[i]->n.orthogonal().normalized();
|
|
axes[1] = g_pContacts[i]->n ^ axes[0];
|
|
for(j=0;j<2;j++) for(k=0;k<2;k++) mtx[j][k] = axes[j]*g_pContacts[i]->K*axes[k];
|
|
matrixf(2,2,0,mtx[0]).invert();
|
|
g_pContacts[i]->Kinv.SetZero();
|
|
for(j=0;j<2;j++) for(k=0;k<2;k++)
|
|
g_pContacts[i]->Kinv += dotproduct_matrix(axes[j],axes[k],mtx1)*mtx[j][k];
|
|
dotproduct_matrix(g_pContacts[i]->n, g_pContacts[i]->n, g_pContacts[i]->C) *= -1.0f;
|
|
g_pContacts[i]->C(0,0)+=1.0f; g_pContacts[i]->C(1,1)+=1.0f; g_pContacts[i]->C(2,2)+=1.0f;
|
|
} else if (g_pContacts[i]->flags & contact_constraint_2dof) {
|
|
t = g_pContacts[i]->n*g_pContacts[i]->K*g_pContacts[i]->n;
|
|
dotproduct_matrix(g_pContacts[i]->n, g_pContacts[i]->n, g_pContacts[i]->C);
|
|
(g_pContacts[i]->Kinv = g_pContacts[i]->C) /= t;
|
|
} else if (g_pContacts[i]->friction<0.01f)
|
|
dotproduct_matrix(g_pContacts[i]->n, g_pContacts[i]->n, g_pContacts[i]->C);
|
|
}
|
|
|
|
g_nBodies = nBodies;
|
|
for(i=0,Ebefore=0;i<nBodies;i++) {
|
|
Ebefore += g_pBodies[i]->P*g_pBodies[i]->v + g_pBodies[i]->L*g_pBodies[i]->w;
|
|
g_pBodies[i]->bProcessed = 0; g_pBodies[i]->Eunproj = 0;
|
|
}
|
|
if (Ebefore < nBodies*sqr(pss->minSeparationSpeed))
|
|
Ebefore = nBodies*sqr(pss->minSeparationSpeed);
|
|
|
|
nMaxIters = pss->nMaxMCiters;
|
|
/*if (pss->nMaxMCiters!=pss->nMaxMCitersHopeless) {
|
|
// check if any body contacts with at least 2 bodies that are more than 50 times heavier than it
|
|
for(i=0;i<g_nContacts;i++) {
|
|
iop = isneg(g_pContacts[i]->pbody[0]->Minv-g_pContacts[i]->pbody[1]->Minv);
|
|
if (g_pContacts[i]->pbody[iop]->Minv > g_pContacts[i]->pbody[iop^1]->Minv*pss->maxMassRatioMC) {
|
|
if (!g_pContacts[i]->pbody[iop]->bProcessed)
|
|
g_pContacts[i]->pbody[iop]->bProcessed = (int)g_pContacts[i]->pbody[iop^1];
|
|
else if (g_pContacts[i]->pbody[iop]->bProcessed != (int)g_pContacts[i]->pbody[iop^1])
|
|
break;
|
|
}
|
|
}
|
|
if (i<g_nContacts)
|
|
nMaxIters = pss->nMaxMCitersHopeless;
|
|
else { // calculate maximum body 'level' in the contact graph
|
|
float minMinv = 1E10f;
|
|
for(i=0;i<nBodies;i++) minMinv = min(minMinv,g_pBodies[i]->Minv);
|
|
minMinv = minMinv*1.001f+0.001f;
|
|
for(i=iend=0;i<nBodies;i++) g_pBodies[i]->bProcessed = -isneg(g_pBodies[i]->Minv-minMinv);
|
|
do {
|
|
for(i=bBounced=0;i<g_nContacts;i++) {
|
|
if ((g_pContacts[i]->pbody[0]->bProcessed & g_pContacts[i]->pbody[0]->bProcessed)==-1)
|
|
bBounced = 1;
|
|
else if ((g_pContacts[i]->pbody[0]->bProcessed | g_pContacts[i]->pbody[0]->bProcessed)==-1) {
|
|
iop = g_pContacts[i]->pbody[0]->bProcessed>>31 & 1;
|
|
iend = max(iend, g_pContacts[i]->pbody[iop^1]->bProcessed = g_pContacts[i]->pbody[iop]->bProcessed+1);
|
|
}
|
|
}
|
|
} while (bBounced);
|
|
if (iend>pss->nMaxStackSizeMC)
|
|
nMaxIters = pss->nMaxMCitersHopeless;
|
|
}
|
|
}*/
|
|
for(i=0;i<nBodies;i++) g_pBodies[i]->bProcessed = 0;
|
|
|
|
if (g_bUsePreCG && g_nContacts<20) {
|
|
FRAME_PROFILER( "PreCG",GetISystem(),PROFILE_PHYSICS );
|
|
|
|
real a,b,r2,r2new,pAp,vmax,vdiff;
|
|
|
|
// try to drive all contact velocities to zero by using conjugate gradient
|
|
for(i=0,r2=0,vmax=0;i<g_nContacts;i++) {
|
|
body0 = g_pContacts[i]->pbody[0]; body1 = g_pContacts[i]->pbody[1];
|
|
if (!(g_pContacts[i]->flags & contact_angular)) {
|
|
r0 = g_pContacts[i]->pt[0]-body0->pos; r1 = g_pContacts[i]->pt[1]-body1->pos;
|
|
dp = body0->v+(body0->w^r0) - body1->v-(body1->w^r1);
|
|
} else dp = body0->w-body1->w;
|
|
g_pContacts[i]->dP = g_pContacts[i]->r = g_pContacts[i]->vreq - g_pContacts[i]->C*dp;
|
|
g_pContacts[i]->P.zero(); r2 += g_pContacts[i]->r.len2(); vmax = max((float)vmax,g_pContacts[i]->r.len2());
|
|
}
|
|
iter = g_nContacts*6;
|
|
|
|
do {
|
|
for(i=0;i<nBodies;i++) { g_pBodies[i]->Fcollision.zero(); g_pBodies[i]->Tcollision.zero(); }
|
|
for(i=0;i<g_nContacts;i++) {
|
|
body0 = g_pContacts[i]->pbody[0]; body1 = g_pContacts[i]->pbody[1];
|
|
if (!(g_pContacts[i]->flags & contact_angular)) {
|
|
r0 = g_pContacts[i]->pt[0]-body0->pos; r1 = g_pContacts[i]->pt[1]-body1->pos;
|
|
body0->Fcollision += g_pContacts[i]->dP; body0->Tcollision += r0^g_pContacts[i]->dP;
|
|
body1->Fcollision -= g_pContacts[i]->dP; body1->Tcollision -= r1^g_pContacts[i]->dP;
|
|
} else {
|
|
body0->Tcollision += g_pContacts[i]->dP; body1->Tcollision -= g_pContacts[i]->dP;
|
|
}
|
|
}
|
|
for(i=0;i<g_nContacts;i++) {
|
|
body0 = g_pContacts[i]->pbody[0]; body1 = g_pContacts[i]->pbody[1];
|
|
if (!(g_pContacts[i]->flags & contact_angular)) {
|
|
r0 = g_pContacts[i]->pt[0]-body0->pos; r1 = g_pContacts[i]->pt[1]-body1->pos;
|
|
dp = body0->Fcollision*body0->Minv + (body0->Iinv*body0->Tcollision^r0);
|
|
dp -= body1->Fcollision*body1->Minv + (body1->Iinv*body1->Tcollision^r1);
|
|
} else
|
|
dp = body0->Iinv*body0->Tcollision - body1->Iinv*body1->Tcollision;
|
|
g_pContacts[i]->vrel = g_pContacts[i]->C*dp;
|
|
}
|
|
|
|
for(i=0,pAp=0;i<g_nContacts;i++)
|
|
pAp += g_pContacts[i]->vrel*g_pContacts[i]->dP;
|
|
if (sqr(pAp)<1E-30) break;
|
|
a = r2/pAp;
|
|
for(i=0,r2new=0;i<g_nContacts;i++) {
|
|
r2new += (g_pContacts[i]->r -= g_pContacts[i]->vrel*a).len2();
|
|
g_pContacts[i]->P += g_pContacts[i]->dP*a;
|
|
}
|
|
if (r2new>r2*500)
|
|
break;
|
|
b = r2new/r2; r2=r2new;
|
|
for(i=0,vmax=0;i<g_nContacts;i++) {
|
|
(g_pContacts[i]->dP*=b)+=g_pContacts[i]->r;
|
|
vmax = max((float)vmax,g_pContacts[i]->r.len2());
|
|
}
|
|
} while (--iter && vmax>sqr(e));
|
|
|
|
for(i=0;i<nBodies;i++) { g_pBodies[i]->Fcollision.zero(); g_pBodies[i]->Tcollision.zero(); }
|
|
for(i=0;i<g_nContacts;i++) {
|
|
body0 = g_pContacts[i]->pbody[0]; body1 = g_pContacts[i]->pbody[1];
|
|
if (!(g_pContacts[i]->flags & contact_angular)) {
|
|
body0->Fcollision += g_pContacts[i]->P; body0->Tcollision += g_pContacts[i]->pt[0]-body0->pos ^ g_pContacts[i]->P;
|
|
body1->Fcollision -= g_pContacts[i]->P; body1->Tcollision -= g_pContacts[i]->pt[1]-body1->pos ^ g_pContacts[i]->P;
|
|
} else {
|
|
body0->Tcollision += g_pContacts[i]->P; body1->Tcollision -= g_pContacts[i]->P;
|
|
}
|
|
}
|
|
for(i=0,Eafter=0;i<nBodies;i++)
|
|
Eafter += (g_pBodies[i]->P+g_pBodies[i]->Fcollision)*(g_pBodies[i]->v+g_pBodies[i]->Fcollision*g_pBodies[i]->Minv) +
|
|
(g_pBodies[i]->L+g_pBodies[i]->Tcollision)*(g_pBodies[i]->w+g_pBodies[i]->Iinv*g_pBodies[i]->Tcollision);
|
|
for(i=0;i<g_nContacts;i++) {
|
|
n = g_pContacts[i]->n; dPn = g_pContacts[i]->P*n; dPtang = (g_pContacts[i]->P-n*dPn).len2();
|
|
if (!(g_pContacts[i]->flags & contact_angular)) {
|
|
dp = g_pContacts[i]->P*(g_pContacts[i]->pbody[0]->Minv+g_pContacts[i]->pbody[1]->Minv);
|
|
vdiff = -0.004f;
|
|
} else {
|
|
dp = g_pContacts[i]->pbody[0]->Iinv*g_pContacts[i]->P + g_pContacts[i]->pbody[1]->Iinv*g_pContacts[i]->P;
|
|
vdiff = -0.015f;
|
|
}
|
|
if (!(g_pContacts[i]->flags & contact_constraint) &&
|
|
(dp*n<-0.05f || // allow to pull bodies at contacts slightly
|
|
dPtang>sqr(dPn*g_pContacts[i]->friction)+0.001f ||
|
|
(g_pContacts[i]->r+g_pContacts[i]->vreq)*n<vdiff))
|
|
break;
|
|
}
|
|
|
|
if (i==g_nContacts && Eafter<Ebefore*1.5f && vmax<sqr(0.01)) { // conjugate gradient yielded acceptable results, apply these impulses and quit
|
|
for(i=0;i<nBodies;i++) {
|
|
g_pBodies[i]->P += g_pBodies[i]->Fcollision; g_pBodies[i]->L += g_pBodies[i]->Tcollision;
|
|
if (g_pBodies[i]->M>0) {
|
|
g_pBodies[i]->v = g_pBodies[i]->P*g_pBodies[i]->Minv; g_pBodies[i]->w = g_pBodies[i]->Iinv*g_pBodies[i]->L;
|
|
}
|
|
g_pBodies[i]->Fcollision *= rtime_interval; g_pBodies[i]->Tcollision *= rtime_interval;
|
|
}
|
|
for(i=0;i<g_nContacts;i++) {
|
|
g_pContacts[i]->vrel = g_pContacts[i]->vreq-g_pContacts[i]->r;
|
|
g_pContacts[i]->Pspare = 1.0f; // indicates that contact is sticky
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
for(i=0; i<g_nContacts; i++) {
|
|
g_Contacts[i].r0 = g_pContacts[i]->pt[0]-g_pContacts[i]->pbody[0]->pos;
|
|
g_Contacts[i].r1 = g_pContacts[i]->pt[1]-g_pContacts[i]->pbody[1]->pos;
|
|
g_Contacts[i].K = g_pContacts[i]->K;
|
|
g_Contacts[i].n = g_pContacts[i]->n;
|
|
g_Contacts[i].vreq = g_pContacts[i]->vreq;
|
|
g_Contacts[i].Pspare = g_pContacts[i]->Pspare;
|
|
g_Contacts[i].flags = g_pContacts[i]->flags;
|
|
g_Contacts[i].friction = g_pContacts[i]->friction;
|
|
g_Contacts[i].iCount = g_pContacts[i]->iCount;
|
|
g_Contacts[i].iCountDst = ((entity_contact*)((char*)g_pContacts[i]->pBounceCount-
|
|
((char*)&g_pContacts[i]->iCount-(char*)g_pContacts[i])))->bProcessed;
|
|
}
|
|
for(i=0;i<nBodies;i++) {
|
|
g_pBodies[i]->Fcollision = g_pBodies[i]->P; g_pBodies[i]->Tcollision = g_Bodies[i].L = g_pBodies[i]->L;
|
|
g_Bodies[i].v = g_pBodies[i]->v; g_Bodies[i].w = g_pBodies[i]->w;
|
|
g_Bodies[i].Minv = g_pBodies[i]->Minv; g_Bodies[i].Iinv = g_pBodies[i]->Iinv; g_Bodies[i].M = g_pBodies[i]->M;
|
|
}
|
|
|
|
iter=nBounces=0; Eafter = 0;
|
|
|
|
//bBounced=1; if (false)
|
|
{
|
|
FRAME_PROFILER( "LCPMC",GetISystem(),PROFILE_PHYSICS );
|
|
|
|
do {
|
|
bBounced = 0;
|
|
//istep = ((iter&1)<<1)-1;
|
|
//istart = g_nContacts-1 & -(iter&1^1);
|
|
//iend = (g_nContacts+1 & -(iter&1))-1;
|
|
istart=0; iend=g_nContacts; istep=1;
|
|
|
|
for(i=istart; i!=iend; i+=istep) {
|
|
if (g_Contacts[i].iCount >= (g_Contacts[i].flags & contact_count_mask)) {
|
|
//body0 = g_Contacts[i]->pbody[0]; body1 = g_pContacts[i]->pbody[1];
|
|
hbody0 = g_Bodies+g_Contacts[i].iBody[0]; hbody1 = g_Bodies+g_Contacts[i].iBody[1];
|
|
n = g_Contacts[i].n;
|
|
if (!(g_Contacts[i].flags & contact_angular)) {
|
|
//r0 = g_pContacts[i]->pt[0]-body0->pos; r1 = g_pContacts[i]->pt[1]-body1->pos;
|
|
r0 = g_Contacts[i].r0; r1 = g_Contacts[i].r1;
|
|
dp = hbody0->v+(hbody0->w^r0) - hbody1->v-(hbody1->w^r1);
|
|
//g_pContacts[i]->vrel =
|
|
} else
|
|
dp = hbody0->w-hbody1->w;
|
|
dp -= g_Contacts[i].vreq;
|
|
if (g_Contacts[i].flags & contact_use_C)
|
|
dp = g_pContacts[i]->C*dp;
|
|
bContactBounced = 0;
|
|
|
|
if (g_Contacts[i].flags & contact_constraint) {
|
|
if ((g_pContacts[i]->C*dp).len2()>sqr(e)) {
|
|
dP = g_pContacts[i]->Kinv*-dp;
|
|
bContactBounced = 1;
|
|
}
|
|
} else if (!(g_Contacts[i].flags & contact_wheel)) {
|
|
if ((vrel=dp*n)<0 &&
|
|
(isneg(e-fabs_tpl(vrel)) | isneg(0.0001f-g_Contacts[i].Pspare) & isneg(sqr(pss->minSeparationSpeed)-(dp-n*vrel).len2())))
|
|
//if ((vrel=dp*n)<0 && (vrel<-0.003 || g_pContacts[i]->Pspare>0.0001f && (dp-n*vrel).len2()>sqr(pss->minSeparationSpeed)))
|
|
{
|
|
if (g_Contacts[i].friction>0.01f) {
|
|
dP = dp*(-dp.len2()/(dp*g_Contacts[i].K*dp));
|
|
g_Contacts[i].Pspare += dPn=(dP*n)*g_Contacts[i].friction;
|
|
dPtang = sqrt_tpl(max(0.0f,dP.len2()-sqr(dP*n)));
|
|
g_Contacts[i].Pspare -= dPtang;
|
|
if (g_Contacts[i].Pspare<0) { // friction cannot stop sliding
|
|
dp += (dp-n*vrel)*((g_Contacts[i].Pspare)/dPtang); // remove part of dp that friction cannot stop
|
|
Kdp = g_Contacts[i].K*dp;
|
|
if (sqr(Kdp*n) < Kdp.len2()*0.04f) // switch to frictionless contact in dangerous cases
|
|
dP = n*-vrel/(n*g_Contacts[i].K*n);
|
|
else
|
|
dP = dp*-vrel/(n*Kdp); // apply impulse along dp so that it stops normal component
|
|
g_Contacts[i].Pspare = 0;
|
|
}
|
|
} else
|
|
dP = n*-vrel/(n*g_Contacts[i].K*n);
|
|
bContactBounced = 1;
|
|
}
|
|
} else if (dp.len2()>sqr(pss->minSeparationSpeed) && g_Contacts[i].Pspare>g_pContacts[i]->pbody[0]->M*0.01f) {
|
|
dP = dp*(-dp.len2()/(dp*g_Contacts[i].K*dp));
|
|
dPn = (dP*n)*g_Contacts[i].friction;
|
|
dPtang = sqrt_tpl(max(0.0f,dP.len2()-sqr(dP*n)));
|
|
if (dPtang > dPn*1.01f) {
|
|
if (g_Contacts[i].Pspare*0.5f < dPtang-dPn) {
|
|
dP *= g_Contacts[i].Pspare*0.5f/(dPtang-dPn);
|
|
g_Contacts[i].Pspare *= 0.5f;
|
|
} else
|
|
g_Contacts[i].Pspare -= dPtang-dPn;
|
|
dP -= n*min(0.0f,dP*n);
|
|
}
|
|
bContactBounced = 1;
|
|
}
|
|
|
|
if (bContactBounced) {
|
|
if (g_Contacts[i].flags & contact_use_C)
|
|
dP = g_pContacts[i]->C*dP;
|
|
if (!(g_Contacts[i].flags & contact_angular)) {
|
|
hbody0->v += dP*hbody0->Minv; hbody0->w += hbody0->Iinv*(dp=r0^dP); hbody0->L += dp;
|
|
hbody1->v -= dP*hbody1->Minv; hbody1->w -= hbody1->Iinv*(dp=r1^dP); hbody1->L -= dp;
|
|
} else {
|
|
hbody0->w += hbody0->Iinv*dP; hbody0->L += dP;
|
|
hbody1->w -= hbody1->Iinv*dP; hbody1->L -= dP;
|
|
}
|
|
g_Contacts[g_Contacts[i].iCountDst].iCount++;
|
|
/*if (!(g_Contacts[i].flags & contact_angular)) {
|
|
body0->P+=dP; body0->L+=r0^dP; body1->P-=dP; body1->L-=r1^dP;
|
|
} else {
|
|
body0->L+=dP; body1->L-=dP;
|
|
}
|
|
if (body0->M>0) { body0->v=body0->P*body0->Minv; body0->w=body0->Iinv*body0->L; }
|
|
if (body1->M>0) { body1->v=body1->P*body1->Minv; body1->w=body1->Iinv*body1->L; }
|
|
g_pContacts[i]->P += dP;
|
|
(*g_pContacts[i]->pBounceCount)++;*/
|
|
bBounced++; nBounces++;
|
|
}
|
|
}
|
|
g_Contacts[i].iCount = 0;
|
|
}
|
|
|
|
for(i=0,Eafter=0; i<nBodies; i++)
|
|
Eafter += g_Bodies[i].v.len2()*g_Bodies[i].M + g_Bodies[i].L*g_Bodies[i].w;
|
|
nBounces += g_nContacts-bBounced >> 4;
|
|
|
|
} while (bBounced && nBounces<nMaxIters && Eafter<Ebefore*3.0f);
|
|
|
|
for(i=0; i<nBodies; i++) {
|
|
g_pBodies[i]->P = (g_pBodies[i]->v=g_Bodies[i].v)*g_pBodies[i]->M;
|
|
g_pBodies[i]->L = g_pBodies[i]->q*(g_pBodies[i]->Ibody*(!g_pBodies[i]->q*(g_pBodies[i]->w = g_Bodies[i].w)));
|
|
}
|
|
for(i=0; i<g_nContacts; i++)
|
|
g_pContacts[i]->Pspare = g_Contacts[i].Pspare;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
if (bBounced && pss->nMaxLCPCGiters>0) { // use several iterations of CG solver, solving only for non-separating contacts
|
|
unsigned int iClass;
|
|
int cgiter,bStateChanged,n1dofContacts,n2dofContacts,nAngContacts,nFric0Contacts,nFricInfContacts,
|
|
nContacts,iSortedContacts[6],flags,bNoImprovement;
|
|
entity_contact *pContacts[sizeof(g_pContacts)/sizeof(g_pContacts[0])];
|
|
real a,b,r2,r2new,pAp;
|
|
float vmax=0;
|
|
vectorf vreq;
|
|
e = pss->accuracyLCPCG;
|
|
|
|
// prepare for CG solver: calculate target residuals (they will be used as starting residuals during each iteration) and vrel's
|
|
for(i=bBounced=0; i<g_nContacts; i++) {
|
|
body0 = g_pContacts[i]->pbody[0]; body1 = g_pContacts[i]->pbody[1];
|
|
//g_pContacts[i]->Pspare = 0;//g_pContacts[i]->flags&contact_angular ? 0:1;
|
|
if (!(g_pContacts[i]->flags & contact_angular)) {
|
|
r0 = g_pContacts[i]->pt[0]-body0->pos; r1 = g_pContacts[i]->pt[1]-body1->pos;
|
|
g_pContacts[i]->vrel = body0->v+(body0->w^r0) - body1->v-(body1->w^r1);
|
|
} else
|
|
g_pContacts[i]->vrel = body0->w-body1->w;
|
|
vreq = g_pContacts[i]->vreq;
|
|
if (g_pContacts[i]->flags & contact_use_C)
|
|
g_pContacts[i]->vrel = g_pContacts[i]->C*g_pContacts[i]->vrel;
|
|
|
|
if (g_pContacts[i]->flags & contact_wheel) {
|
|
if (g_pContacts[i]->Pspare>g_pContacts[i]->pbody[0]->M*0.01f)
|
|
g_pContacts[i]->flags = (g_pContacts[i]->flags & contact_use_C) | contact_constraint_3dof;
|
|
else
|
|
g_pContacts[i]->flags = contact_count_mask; // don't solve for this contact
|
|
}
|
|
|
|
// remove constraint flags from contacts with counters, since constraints cannot to removed from solving process
|
|
g_pContacts[i]->flags &= contact_count_mask | (g_pContacts[i]->flags&contact_count_mask)-1>>31;
|
|
if (g_pContacts[i]->flags & contact_constraint_1dof)
|
|
g_pContacts[i]->r0 = (g_pContacts[i]->vrel -= g_pContacts[i]->n*(g_pContacts[i]->vrel*g_pContacts[i]->n));
|
|
else if (g_pContacts[i]->flags&contact_constraint_2dof || !(g_pContacts[i]->flags&contact_constraint_3dof || g_pContacts[i]->Pspare>0)) {
|
|
g_pContacts[i]->r0(g_pContacts[i]->n*g_pContacts[i]->vrel,0,0); vreq(g_pContacts[i]->n*vreq,0,0);
|
|
g_pContacts[i]->vrel = g_pContacts[i]->n*g_pContacts[i]->r0.x;
|
|
g_pContacts[i]->Kinv.SetZero();
|
|
g_pContacts[i]->Kinv(0,0) = 1.0f/(g_pContacts[i]->n*g_pContacts[i]->K*g_pContacts[i]->n);
|
|
} else {
|
|
g_pContacts[i]->r0 = g_pContacts[i]->vrel;
|
|
if (!(g_pContacts[i]->flags & contact_constraint))
|
|
(g_pContacts[i]->Kinv = g_pContacts[i]->K).Invert(); // for constraints it's computed earlier
|
|
}
|
|
g_pContacts[i]->r0.Flip();
|
|
|
|
if (g_pContacts[i]->flags & contact_constraint || g_pContacts[i]->vrel*g_pContacts[i]->n<0) {
|
|
iop = g_pContacts[i]->flags>>contact_angular_log2&1;
|
|
bBounced += isneg(sqr(body0->softness[iop]+body1->softness[iop])*0.25f - (g_pContacts[i]->r0+vreq).len2()*sqr(time_interval));
|
|
}
|
|
g_pContacts[i]->flags &= ~contact_solve_for;
|
|
}
|
|
if (bBounced) {
|
|
{FRAME_PROFILER( "LCPCG",GetISystem(),PROFILE_PHYSICS );
|
|
|
|
cgiter = pss->nMaxLCPCGiters;
|
|
for(i=0;i<nBodies;i++) {
|
|
g_infos[i].Fcollision = g_pBodies[i]->Fcollision;
|
|
g_infos[i].Tcollision = g_pBodies[i]->Tcollision;
|
|
}
|
|
|
|
do {
|
|
// sort contacts for the current iteration in the following way:
|
|
// 1dof angular (always remove v-n*(v*n)) | angular
|
|
// 2dof angular constraints (always remove n*v) | angular
|
|
// angular limits (remove n*v if <0) | angular
|
|
// frictionless contacts (remove n*v if <0) | positional
|
|
// infinite friction contacts (remove v if n*v<0) | positional
|
|
// 3dof constraints (always remove v) | positional
|
|
for(i=0;i<6;i++) iSortedContacts[i]=0;
|
|
for(i=bStateChanged=0; i<g_nContacts; i++) {
|
|
flags = g_pContacts[i]->flags;
|
|
if (!(g_pContacts[i]->flags&contact_count_mask)) {
|
|
g_pContacts[i]->flags |= contact_solve_for;
|
|
if (g_pContacts[i]->flags & contact_constraint_1dof) iSortedContacts[0]++;
|
|
else if (g_pContacts[i]->flags & contact_constraint_2dof) iSortedContacts[1]++;
|
|
else if (g_pContacts[i]->flags & contact_constraint_3dof) iSortedContacts[5]++;
|
|
else {
|
|
if (g_pContacts[i]->vrel*g_pContacts[i]->n<0) {
|
|
if (g_pContacts[i]->flags & contact_angular) iSortedContacts[2]++;
|
|
else if (g_pContacts[i]->Pspare>0) iSortedContacts[4]++;
|
|
else iSortedContacts[3]++;
|
|
} else
|
|
g_pContacts[i]->flags &= ~contact_solve_for;
|
|
bStateChanged += iszero((flags^g_pContacts[i]->flags) & contact_solve_for)^1;
|
|
}
|
|
}
|
|
}
|
|
if (!bStateChanged && vmax<sqr(e))
|
|
break;
|
|
iter = iSortedContacts[0]*2 + iSortedContacts[1]+iSortedContacts[2]+iSortedContacts[3] + (iSortedContacts[4]+iSortedContacts[5])*3;
|
|
for(i=1;i<6;i++) iSortedContacts[i]+=iSortedContacts[i-1];
|
|
n1dofContacts=iSortedContacts[0]; n2dofContacts=iSortedContacts[1]; nAngContacts=iSortedContacts[2];
|
|
nFric0Contacts=iSortedContacts[3];nFricInfContacts=iSortedContacts[4]; nContacts=iSortedContacts[5];
|
|
for(i=0,r2=0; i<g_nContacts; i++) {
|
|
if (g_pContacts[i]->flags & contact_constraint_1dof) iClass = 0;
|
|
else if (g_pContacts[i]->flags & contact_constraint_2dof) iClass = 1;
|
|
else if (g_pContacts[i]->flags & contact_constraint_3dof) iClass = 5;
|
|
else if (g_pContacts[i]->flags & contact_solve_for) {
|
|
if (g_pContacts[i]->flags & contact_angular) iClass = 2;
|
|
else if (g_pContacts[i]->Pspare>0) iClass = 4;
|
|
else iClass = 3;
|
|
} else
|
|
continue;
|
|
pContacts[--iSortedContacts[iClass]] = g_pContacts[i];
|
|
g_pContacts[i]->vrel = g_pContacts[i]->Kinv*(g_pContacts[i]->r = g_pContacts[i]->r0);
|
|
if (g_pContacts[i]->flags & contact_use_C)
|
|
g_pContacts[i]->vrel = g_pContacts[i]->C*g_pContacts[i]->vrel;
|
|
r2 += g_pContacts[i]->vrel*g_pContacts[i]->r0;
|
|
g_pContacts[i]->dP = iClass-1u<3u ? g_pContacts[i]->n*(g_pContacts[i]->dPn=g_pContacts[i]->vrel.x) : g_pContacts[i]->vrel;
|
|
g_pContacts[i]->P.zero();
|
|
}
|
|
if (!(cgiter==1 || !bStateChanged)) {
|
|
iter = min(iter,pss->nMaxLCPCGsubiters);
|
|
if (iter*nContacts > pss->nMaxLCPCGmicroiters)
|
|
iter = max(5,pss->nMaxLCPCGmicroiters/nContacts);
|
|
} else {
|
|
iter = min(iter*2,pss->nMaxLCPCGsubitersFinal);
|
|
if (iter*nContacts > pss->nMaxLCPCGmicroitersFinal)
|
|
iter = max(10,pss->nMaxLCPCGmicroitersFinal/nContacts);
|
|
}
|
|
bNoImprovement = 0;
|
|
|
|
do {
|
|
for(i=0;i<nBodies;i++) { g_pBodies[i]->Fcollision.zero(); g_pBodies[i]->Tcollision.zero(); }
|
|
for(i=0;i<nAngContacts;i++) { // angular contacts
|
|
pContacts[i]->pbody[0]->Tcollision += pContacts[i]->dP;
|
|
pContacts[i]->pbody[1]->Tcollision -= pContacts[i]->dP;
|
|
} for(;i<nContacts;i++) { // positional contacts
|
|
body0 = pContacts[i]->pbody[0]; body1 = pContacts[i]->pbody[1];
|
|
r0 = pContacts[i]->pt[0]-body0->pos; r1 = pContacts[i]->pt[1]-body1->pos;
|
|
body0->Fcollision += pContacts[i]->dP; body0->Tcollision += r0^pContacts[i]->dP;
|
|
body1->Fcollision -= pContacts[i]->dP; body1->Tcollision -= r1^pContacts[i]->dP;
|
|
}
|
|
|
|
for(i=0;i<nAngContacts;i++) { // angular contacts
|
|
body0 = pContacts[i]->pbody[0]; body1 = pContacts[i]->pbody[1];
|
|
pContacts[i]->vrel = body0->Iinv*body0->Tcollision - body1->Iinv*body1->Tcollision;
|
|
} for(;i<nContacts;i++) { // positional contacts
|
|
body0 = pContacts[i]->pbody[0]; body1 = pContacts[i]->pbody[1];
|
|
r0 = pContacts[i]->pt[0]-body0->pos; r1 = pContacts[i]->pt[1]-body1->pos;
|
|
pContacts[i]->vrel = body0->Fcollision*body0->Minv + (body0->Iinv*body0->Tcollision^r0);
|
|
pContacts[i]->vrel -= body1->Fcollision*body1->Minv + (body1->Iinv*body1->Tcollision^r1);
|
|
}
|
|
|
|
pAp = 0;
|
|
for(i=0;i<n1dofContacts;i++) { // 1-dof contacts (remove 1 axis from vrel)
|
|
pContacts[i]->vrel -= pContacts[i]->n*(pContacts[i]->vrel*pContacts[i]->n);
|
|
pAp += pContacts[i]->vrel*pContacts[i]->dP;
|
|
} for(;i<nFric0Contacts;i++) { // general 2-dof contacts (remove 2 axes from vrel - leave 1)
|
|
pContacts[i]->vrel.x = pContacts[i]->n*pContacts[i]->vrel;
|
|
pAp += pContacts[i]->vrel.x*pContacts[i]->dPn;
|
|
} for(;i<nContacts;i++) // general 3-dof contacts (leave vrel untouched - all directions need to be 0ed)
|
|
pAp += pContacts[i]->vrel*pContacts[i]->dP;
|
|
|
|
a = min((real)50.0,r2/max((real)1E-10,pAp)); r2new=0;
|
|
i = nFric0Contacts&nFric0Contacts-nContacts>>31;
|
|
if (nFric0Contacts-n1dofContacts<nContacts) do { // contacts with 3(and 2)-components vel
|
|
pContacts[i]->vrel = pContacts[i]->Kinv*(pContacts[i]->r -= pContacts[i]->vrel*a);
|
|
if (pContacts[i]->flags & contact_use_C)
|
|
pContacts[i]->vrel = pContacts[i]->C*pContacts[i]->vrel;
|
|
r2new += pContacts[i]->vrel*pContacts[i]->r;
|
|
pContacts[i]->P += pContacts[i]->dP*a;
|
|
i = i+1&i+1-nContacts>>31;
|
|
} while(i!=n1dofContacts);
|
|
for(;i<nFric0Contacts;i++) { // contacts with 1-component vel
|
|
pContacts[i]->vrel.x = pContacts[i]->Kinv(0,0)*(pContacts[i]->r.x -= pContacts[i]->vrel.x*a);
|
|
r2new += pContacts[i]->vrel.x*pContacts[i]->r.x;
|
|
pContacts[i]->P.x += pContacts[i]->dPn*a;
|
|
}
|
|
|
|
bNoImprovement = max(0,bNoImprovement-sgnnz(r2-r2new - r2*pss->minLCPCGimprovement));
|
|
b = r2new/r2; r2=r2new; vmax=0;
|
|
i = nFric0Contacts&nFric0Contacts-nContacts>>31;
|
|
if (nFric0Contacts-n1dofContacts<nContacts) do { // contacts with 3(and 2)-components vel
|
|
(pContacts[i]->dP*=b) += pContacts[i]->vrel;
|
|
vmax = max(vmax,pContacts[i]->r.len2());
|
|
i = i+1&i+1-nContacts>>31;
|
|
} while(i!=n1dofContacts);
|
|
for(;i<nFric0Contacts;i++) { // contacts with 1-component vel
|
|
(pContacts[i]->dPn*=b) += pContacts[i]->vrel.x;
|
|
pContacts[i]->dP = pContacts[i]->n*pContacts[i]->dPn;
|
|
vmax = max(vmax,sqr(pContacts[i]->r.x));
|
|
}
|
|
|
|
} while (--iter && vmax>sqr(e) && !(bNoImprovement>pss->nMaxLCPCGFruitlessIters && vmax<sqr(pss->accuracyLCPCGnoimprovement)));
|
|
|
|
for(i=n1dofContacts;i<nFric0Contacts;i++) pContacts[i]->P = pContacts[i]->n*pContacts[i]->P.x;
|
|
if (!bStateChanged)
|
|
break;
|
|
|
|
// calculate how the impulses affect the contacts excluded from this iteration
|
|
for(i=0;i<nBodies;i++) { g_pBodies[i]->Fcollision.zero(); g_pBodies[i]->Tcollision.zero(); }
|
|
for(i=0;i<nAngContacts;i++) { // angular contacts
|
|
pContacts[i]->pbody[0]->Tcollision += pContacts[i]->P;
|
|
pContacts[i]->pbody[1]->Tcollision -= pContacts[i]->P;
|
|
} for(;i<nContacts;i++) { // positional contacts
|
|
body0 = pContacts[i]->pbody[0]; body1 = pContacts[i]->pbody[1];
|
|
r0 = pContacts[i]->pt[0]-body0->pos; r1 = pContacts[i]->pt[1]-body1->pos;
|
|
body0->Fcollision += pContacts[i]->P; body0->Tcollision += r0^pContacts[i]->P;
|
|
body1->Fcollision -= pContacts[i]->P; body1->Tcollision -= r1^pContacts[i]->P;
|
|
}
|
|
for(i=0;i<g_nContacts;i++) if (!(g_pContacts[i]->flags & (contact_solve_for|contact_count_mask))) {
|
|
body0 = g_pContacts[i]->pbody[0]; body1 = g_pContacts[i]->pbody[1];
|
|
if (!(g_pContacts[i]->flags & contact_angular)) {
|
|
r0 = g_pContacts[i]->pt[0]-body0->pos; r1 = g_pContacts[i]->pt[1]-body1->pos;
|
|
g_pContacts[i]->vrel = body0->v + body0->Fcollision*body0->Minv + (body0->w+body0->Iinv*body0->Tcollision ^ r0);
|
|
g_pContacts[i]->vrel -= body1->v + body1->Fcollision*body1->Minv + (body1->w+body1->Iinv*body1->Tcollision ^ r1);
|
|
} else
|
|
g_pContacts[i]->vrel = body0->w+body0->Iinv*body0->Tcollision - body1->w-body1->Iinv*body1->Tcollision;
|
|
if (g_pContacts[i]->flags & contact_use_C)
|
|
g_pContacts[i]->vrel = g_pContacts[i]->C*g_pContacts[i]->vrel;
|
|
}
|
|
if (cgiter>2) for(i=n2dofContacts; i<nFricInfContacts; i++)
|
|
pContacts[i]->vrel = -pContacts[i]->P*max(pContacts[i]->pbody[0]->Minv,pContacts[i]->pbody[1]->Minv)-pContacts[i]->n*(e*3);
|
|
else for(i=n2dofContacts; i<nFricInfContacts; i++) // if we reached the last iteration, keep the current 'solve for' contacts unconditionally
|
|
pContacts[i]->vrel = -pContacts[i]->n;
|
|
} while(--cgiter>0);
|
|
|
|
|
|
bBounced = 1;
|
|
if (cgiter<pss->nMaxLCPCGiters) {
|
|
for(i=0,vmax=0;i<nAngContacts;i++) {
|
|
vmax = max(vmax,(pContacts[i]->pbody[0]->Ibody*pContacts[i]->P).len2());
|
|
vmax = max(vmax,(pContacts[i]->pbody[1]->Ibody*pContacts[i]->P).len2());
|
|
} if (_isnan(vmax) || vmax>sqr(pss->maxwCG))
|
|
bBounced = 0;
|
|
for(vmax=0;i<nContacts;i++) {
|
|
vmax = max(vmax,sqr(pContacts[i]->pbody[0]->Minv)*pContacts[i]->P.len2());
|
|
vmax = max(vmax,sqr(pContacts[i]->pbody[1]->Minv)*pContacts[i]->P.len2());
|
|
} if (_isnan(vmax) || vmax>sqr(pss->maxvCG))
|
|
bBounced = 0;
|
|
|
|
if (bBounced) {
|
|
for(;i<nContacts;i++) { // positional contacts
|
|
body0 = pContacts[i]->pbody[0]; body1 = pContacts[i]->pbody[1];
|
|
body0->P += pContacts[i]->P; body0->L += r0^pContacts[i]->P;
|
|
body1->P -= pContacts[i]->P;
|
|
}
|
|
// apply P from the last CG iteration
|
|
for(i=0;i<nAngContacts;i++) { // angular contacts
|
|
pContacts[i]->pbody[0]->L += pContacts[i]->P;
|
|
pContacts[i]->pbody[1]->L -= pContacts[i]->P;
|
|
} for(;i<nContacts;i++) { // positional contacts
|
|
body0 = pContacts[i]->pbody[0]; body1 = pContacts[i]->pbody[1];
|
|
r0 = pContacts[i]->pt[0]-body0->pos; r1 = pContacts[i]->pt[1]-body1->pos;
|
|
body0->P += pContacts[i]->P; body0->L += r0^pContacts[i]->P;
|
|
body1->P -= pContacts[i]->P; body1->L -= r1^pContacts[i]->P;
|
|
}
|
|
for(i=0;i<nBodies;i++) if (g_pBodies[i]->M>0) {
|
|
g_pBodies[i]->v = g_pBodies[i]->P*g_pBodies[i]->Minv; g_pBodies[i]->w = g_pBodies[i]->Iinv*g_pBodies[i]->L;
|
|
}
|
|
} else for(i=0; i<nBodies; i++) {
|
|
j = g_pBodies[i]->bProcessed;
|
|
g_pBodies[i]->Fcollision = g_infos[j].Fcollision;
|
|
g_pBodies[i]->Tcollision = g_infos[j].Tcollision;
|
|
}
|
|
}
|
|
|
|
}//"LCPCG"
|
|
|
|
if (bBounced) {
|
|
FRAME_PROFILER( "LCPCG-unproj",GetISystem(),PROFILE_PHYSICS );
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////
|
|
// now, use a separate solver for unprojections (unproject each body independently)
|
|
int nBuddies,nSandwiches,iMinLevel,iMaxLevel,iLevel;
|
|
buddy_info *pbuddy0,*pbuddy1;
|
|
vectorf vreq;
|
|
float minMinv;
|
|
|
|
for(i=0;i<nBodies;i++) {
|
|
g_pBodies[i]->bProcessed=i; g_infos[i].pbuddy=0; g_infos[i].iLevel=-1;
|
|
g_infos[i].psandwich=0; g_infos[i].pfollower=0; g_infos[i].idUpdate=0;
|
|
}
|
|
// require that the all contacts are grouped by pbody[0]
|
|
for(istart=nBuddies=0; istart<g_nContacts; istart=iend) {
|
|
// sub-group contacts relating to each pbody[0] by pbody[1] (using quick sort)
|
|
for(iend=istart+1; iend<g_nContacts && g_pContacts[iend]->pbody[0]==g_pContacts[istart]->pbody[0]; iend++);
|
|
qsort(g_pContacts, istart,iend-1);
|
|
// for each body: find all its contacting bodies and for each such body get integral vreq (but ignore separating contacts)
|
|
for(i=istart; i<iend; i=j) {
|
|
for(j=i,vreq.zero(),flags=0; j<iend && g_pContacts[j]->pbody[1]==g_pContacts[i]->pbody[1]; j++)
|
|
if (g_pContacts[j]->flags&contact_solve_for && g_pContacts[j]->vreq.len2()>0) {
|
|
vreq += g_pContacts[j]->vreq; flags |= g_pContacts[j]->flags;
|
|
}
|
|
if (flags) {
|
|
g_buddybuf[nBuddies].next = g_infos[bidx0(i)].pbuddy;
|
|
g_buddybuf[nBuddies].iBody = bidx1(i);
|
|
g_buddybuf[nBuddies].vreq = vreq; g_buddybuf[nBuddies].flags = flags;
|
|
g_infos[bidx0(i)].pbuddy = g_buddybuf+nBuddies++;
|
|
g_buddybuf[nBuddies].next = g_infos[bidx1(i)].pbuddy;
|
|
g_buddybuf[nBuddies].iBody = bidx0(i);
|
|
g_buddybuf[nBuddies].vreq = -vreq; g_buddybuf[nBuddies].flags = flags;
|
|
g_infos[bidx1(i)].pbuddy = g_buddybuf+nBuddies++;
|
|
}
|
|
}
|
|
}
|
|
|
|
// for every 2 contacting bodies of each body check if vreqs are conflicting, register sandwich triplet if so (1 inside, 2 outside)
|
|
for(i=nSandwiches=0; i<nBodies; i++)
|
|
for(pbuddy0=g_infos[i].pbuddy; pbuddy0; pbuddy0=pbuddy0->next)
|
|
for(pbuddy1=pbuddy0->next; pbuddy1; pbuddy1=pbuddy1->next)
|
|
if (pbuddy0->vreq*pbuddy1->vreq<0 && !((pbuddy0->flags^pbuddy1->flags)&contact_angular)) {
|
|
g_sandwichbuf[nSandwiches].iMiddle = i;
|
|
g_sandwichbuf[nSandwiches].iBread[0] = pbuddy0->iBody;
|
|
g_sandwichbuf[nSandwiches].iBread[1] = pbuddy1->iBody;
|
|
g_sandwichbuf[nSandwiches].next = g_infos[i].psandwich;
|
|
g_sandwichbuf[nSandwiches].bProcessed = 0;
|
|
g_infos[i].psandwich = g_sandwichbuf+nSandwiches++;
|
|
}
|
|
|
|
// call tracepath for each sandwich with static as 'bread' (if no static - assign a 'pseudo-static')
|
|
for(i=0;i<nBodies;i++) g_infos[i].Minv = g_pBodies[i]->Minv;
|
|
// find the heaviest body participating as bread in any sandwich
|
|
for(i=j=0,minMinv=1E10f; i<nSandwiches; i++) {
|
|
if (g_infos[g_sandwichbuf[i].iBread[0]].Minv<minMinv) minMinv = g_infos[j=g_sandwichbuf[i].iBread[0]].Minv;
|
|
if (g_infos[g_sandwichbuf[i].iBread[1]].Minv<minMinv) minMinv = g_infos[j=g_sandwichbuf[i].iBread[1]].Minv;
|
|
}
|
|
if (minMinv>0) { // if no static participates in sandwich, assign bodies contacting with statics Minv 0 and repeat the procedure
|
|
for(i=0; i<g_nContacts; i++) if (g_pContacts[i]->pbody[0]->Minv*g_pContacts[i]->pbody[1]->Minv==0)
|
|
g_infos[g_pContacts[i]->pbody[g_pContacts[i]->pbody[0]->Minv==0]->bProcessed].Minv = 0;
|
|
for(i=0,minMinv=1E10f; i<nSandwiches; i++) {
|
|
if (g_infos[g_sandwichbuf[i].iBread[0]].Minv<minMinv) minMinv = g_infos[j=g_sandwichbuf[i].iBread[0]].Minv;
|
|
if (g_infos[g_sandwichbuf[i].iBread[1]].Minv<minMinv) minMinv = g_infos[j=g_sandwichbuf[i].iBread[1]].Minv;
|
|
}
|
|
}
|
|
for(i=g_nFollowers=g_nUnprojLoops=0; i<nSandwiches; i++) {
|
|
if (j==g_sandwichbuf[i].iBread[0] || g_infos[g_sandwichbuf[i].iBread[0]].Minv==0) iop=0;
|
|
else if (j==g_sandwichbuf[i].iBread[1] || g_infos[g_sandwichbuf[i].iBread[1]].Minv==0) iop=1;
|
|
else continue;
|
|
g_infos[g_sandwichbuf[i].iBread[iop]].iLevel = 0;
|
|
g_infos[g_sandwichbuf[i].iMiddle].iLevel = max(1,g_infos[g_sandwichbuf[i].iMiddle].iLevel);
|
|
trace_unproj_route(g_sandwichbuf[i].iMiddle, g_sandwichbuf[i].iBread[iop]);
|
|
}
|
|
|
|
// option: since after this moment we are starting to get 'heuristic', maybe we should randomize the order of sandwiches?
|
|
iter = 0;
|
|
do {
|
|
// iteratively select an unprocessed sandwich with the min level body (bread or middle), call trace_unproj_route for it;
|
|
// when the route reaches initialized branches, they will automatically update all previously traced followers
|
|
do {
|
|
for(i=0,j=-1,iMinLevel=0x10000; i<nSandwiches; i++) if (!g_sandwichbuf[i].bProcessed) {
|
|
iLevel = min(g_infos[g_sandwichbuf[i].iMiddle].iLevel&0xFFFF,
|
|
min(g_infos[g_sandwichbuf[i].iBread[0]].iLevel&0xFFFF,g_infos[g_sandwichbuf[i].iBread[1]].iLevel&0xFFFF));
|
|
iop = iLevel-iMinLevel>>31; j = i&iop | j&~iop;
|
|
iMinLevel = min(iMinLevel,iLevel);
|
|
}
|
|
if (j<0) break;
|
|
g_sandwichbuf[j].bProcessed = 1;
|
|
i = (g_infos[g_sandwichbuf[j].iBread[0]].iLevel>>31&1 | g_infos[g_sandwichbuf[j].iBread[1]].iLevel>>31&2 |
|
|
g_infos[g_sandwichbuf[j].iMiddle].iLevel>>31&4) ^ 7;
|
|
if (i==7) { // all participating bodies already have levels
|
|
iop = isneg(g_infos[g_sandwichbuf[j].iBread[1]].iLevel-g_infos[g_sandwichbuf[j].iBread[0]].iLevel); // min level bread
|
|
if (g_infos[g_sandwichbuf[j].iMiddle].iLevel >= g_infos[g_sandwichbuf[j].iBread[iop]].iLevel)
|
|
add_route_follower(g_sandwichbuf[j].iBread[iop], g_sandwichbuf[j].iMiddle); // breadmin -> middle -> breadmax(will be updated)
|
|
} else if (i==3) { // both breads have levels
|
|
g_infos[g_sandwichbuf[j].iMiddle].iLevel = 1;
|
|
if (iMinLevel>1) { // bread0 <- middle=1 -> bread1
|
|
add_route_follower(g_sandwichbuf[j].iMiddle, g_sandwichbuf[j].iBread[0]);
|
|
add_route_follower(g_sandwichbuf[j].iMiddle, g_sandwichbuf[j].iBread[1]);
|
|
} else { // breadmin -> middle=1 -> breadmax
|
|
iop = isneg(g_infos[g_sandwichbuf[j].iBread[1]].iLevel-g_infos[g_sandwichbuf[j].iBread[0]].iLevel); // min level bread
|
|
add_route_follower(g_sandwichbuf[j].iBread[iop], g_sandwichbuf[j].iMiddle);
|
|
}
|
|
} else if (i==4) { // only the middle has level
|
|
g_infos[g_sandwichbuf[j].iBread[0]].iLevel = g_infos[g_sandwichbuf[j].iBread[1]].iLevel =
|
|
g_infos[g_sandwichbuf[j].iMiddle].iLevel+1;
|
|
add_route_follower(g_sandwichbuf[j].iMiddle, g_sandwichbuf[j].iBread[0]);
|
|
add_route_follower(g_sandwichbuf[j].iMiddle, g_sandwichbuf[j].iBread[1]);
|
|
} else if (i!=0) { // either one bread or one bread and the middle have levels
|
|
iop = i>>1&1; // the bread that has level
|
|
g_infos[g_sandwichbuf[j].iMiddle].iLevel = min(g_infos[g_sandwichbuf[j].iMiddle].iLevel&0xFFFF, iMinLevel+1);
|
|
if (i&2) // the middle had level
|
|
add_route_follower(g_sandwichbuf[j].iMiddle, g_sandwichbuf[j].iBread[iop^1]);
|
|
else
|
|
add_route_follower(g_sandwichbuf[j].iBread[iop], g_sandwichbuf[j].iMiddle);
|
|
}
|
|
} while(true);
|
|
if (iter>0) break;
|
|
|
|
// now we have only fully clear sandwiches in isolated branches; as we progress, however, some unprocessed sandwiches might
|
|
// become partially initialized; ignore them - the previous loop will pick them up during the next iteration
|
|
for(i=0;i<nSandwiches;i++)
|
|
if ((g_infos[g_sandwichbuf[i].iMiddle].iLevel & g_infos[g_sandwichbuf[i].iBread[0]].iLevel & g_infos[g_sandwichbuf[i].iBread[1]].iLevel)==-1) {
|
|
g_sandwichbuf[i].bProcessed = 1;
|
|
g_infos[g_sandwichbuf[i].iMiddle].iLevel = 0;
|
|
g_infos[g_sandwichbuf[i].iBread[0]].iLevel = g_infos[g_sandwichbuf[i].iBread[1]].iLevel = 1;
|
|
add_route_follower(g_sandwichbuf[i].iMiddle, g_sandwichbuf[i].iBread[0]);
|
|
add_route_follower(g_sandwichbuf[i].iMiddle, g_sandwichbuf[i].iBread[1]);
|
|
}
|
|
++iter;
|
|
} while(true);
|
|
|
|
// set level to maxlevel+1 for all bodies that still don't have a level assigned
|
|
// (they either didn't form sandwiches, or belonged to sandwiches that had only middles initialized, and didn't belong to routes
|
|
for(i=iMaxLevel=0; i<nBodies; i++) iMaxLevel = max(iMaxLevel,g_infos[i].iLevel);
|
|
for(i=0,iMaxLevel++; i<nBodies; i++) {
|
|
g_infos[i].iLevel = iMaxLevel&g_infos[i].iLevel>>31 | max(g_infos[i].iLevel,0);
|
|
if (g_pBodies[i]->Minv==0)
|
|
g_infos[i].iLevel = 0; // force level 0 to statics (some can accidentally get non-0 level if they don't participate in sandwiches)
|
|
}
|
|
|
|
#ifdef _DEBUG
|
|
for(i=j=0;i<nSandwiches;i++)
|
|
if (g_infos[g_sandwichbuf[i].iMiddle].iLevel>=max(g_infos[g_sandwichbuf[i].iBread[0]].iLevel,g_infos[g_sandwichbuf[i].iBread[1]].iLevel))
|
|
j++;
|
|
#endif
|
|
|
|
// sort body list according to level
|
|
qsort(g_pBodies,0,nBodies-1);
|
|
for(i=0;i<nBodies;i++) {
|
|
g_infos[g_pBodies[i]->bProcessed].idx = i;
|
|
g_infos[g_pBodies[i]->bProcessed].v_unproj.zero(); g_infos[g_pBodies[i]->bProcessed].w_unproj.zero();
|
|
}
|
|
// sort contact list so that each contact gets assigned to the body with the higher level (among the two participating in the contact)
|
|
qsort((entity_contact_unproj**)g_pContacts,0,g_nContacts-1);
|
|
|
|
for(istart=0; istart<g_nContacts; istart=j) {
|
|
// among contacts assigned to a body, select only non-separating with non-zero vreq and use cg to enforce vreq (along its direction only)
|
|
// don't enforce contacts with 0 vreq, since if we violate them, they'll be reported to have vreq during the next step
|
|
body0 = g_pContacts[istart]->pbody[isneg(g_infos[bidx0(istart)].iLevel-g_infos[bidx1(istart)].iLevel)];
|
|
iSortedContacts[0] = iSortedContacts[1] = 0;
|
|
for(j=istart; j<g_nContacts && g_pContacts[j]->pbody[iop = isneg(g_infos[bidx0(j)].iLevel-g_infos[bidx1(j)].iLevel)]==body0; j++)
|
|
if (g_pContacts[j]->flags & contact_solve_for && g_pContacts[j]->vreq.len2()>sqr(0.01f)) {
|
|
g_pContacts[j]->flags = g_pContacts[j]->flags&~contact_bidx | iop<<contact_bidx_log2;
|
|
g_pContacts[j]->vrel = g_pContacts[j]->vreq*(iop*2-1);
|
|
// from now on use dP to store normalized unprojection direction
|
|
if (fabsf(sqr(g_pContacts[j]->vreq*g_pContacts[j]->n)-g_pContacts[j]->vreq.len2()) < g_pContacts[j]->vreq.len2()*0.01f)
|
|
g_pContacts[j]->dP = g_pContacts[j]->n;
|
|
else
|
|
g_pContacts[j]->dP = g_pContacts[j]->vreq.normalized();
|
|
body1 = g_pContacts[j]->pbody[iop^1];
|
|
if (!(g_pContacts[j]->flags & contact_angular)) {
|
|
r0 = g_pContacts[j]->pt[iop]-body0->pos; r1 = g_pContacts[j]->pt[iop^1]-body1->pos;
|
|
g_pContacts[j]->Kinv(0,0) = body0->Minv+g_pContacts[j]->dP*(body0->Iinv*(r0^g_pContacts[j]->dP)^r0);
|
|
if (g_pContacts[j]->Kinv(0,0) < body0->Minv*0.1f) { // contact has near-degenerate Kinv, skip it
|
|
g_pContacts[j]->flags &= ~contact_solve_for; continue;
|
|
}
|
|
g_pContacts[j]->vrel += body0->v+g_infos[body0->bProcessed].v_unproj+(body0->w+g_infos[body0->bProcessed].w_unproj^r0);
|
|
g_pContacts[j]->vrel -= body1->v+g_infos[body1->bProcessed].v_unproj+(body1->w+g_infos[body1->bProcessed].w_unproj^r1);
|
|
iSortedContacts[1]++;
|
|
} else {
|
|
g_pContacts[j]->Kinv(0,0) = g_pContacts[j]->dP*(body0->Iinv*g_pContacts[j]->dP);
|
|
g_pContacts[j]->vrel += body0->w+g_infos[body0->bProcessed].w_unproj - body1->w-g_infos[body1->bProcessed].w_unproj;
|
|
iSortedContacts[0]++;
|
|
}
|
|
} else
|
|
g_pContacts[j]->flags &= ~contact_solve_for;
|
|
nContacts=iSortedContacts[0]+iSortedContacts[1]; nAngContacts=iSortedContacts[1]=iSortedContacts[0]; iSortedContacts[0]=0;
|
|
if (body0->Minv==0)
|
|
continue;
|
|
|
|
for(j=istart; j<g_nContacts && g_pContacts[j]->pbody[iop = isneg(g_infos[bidx0(j)].iLevel-g_infos[bidx1(j)].iLevel)]==body0; j++)
|
|
if (g_pContacts[j]->flags & contact_solve_for) {
|
|
g_pContacts[j]->Kinv(0,0) = 1.0f/g_pContacts[j]->Kinv(0,0);
|
|
g_pContacts[j]->r0.x = g_pContacts[j]->r.x = -(g_pContacts[j]->vrel*g_pContacts[j]->dP);
|
|
g_pContacts[j]->dPn = g_pContacts[j]->r.x*g_pContacts[j]->Kinv(0,0);
|
|
g_pContacts[j]->P.x = 0;
|
|
pContacts[iSortedContacts[g_pContacts[j]->flags>>contact_angular_log2&1^1]++] = g_pContacts[j];
|
|
}
|
|
r2 = ComputeRc(body0,pContacts,nAngContacts,nContacts);
|
|
|
|
// use MINRES to enforce vreq
|
|
iter = nContacts;
|
|
if (iter=nContacts) {
|
|
do {
|
|
body0->Fcollision.zero(); body0->Tcollision.zero();
|
|
for(i=0;i<nAngContacts;i++) // angular contacts
|
|
body0->Tcollision += pContacts[i]->dP*pContacts[i]->dPn;
|
|
for(;i<nContacts;i++) { // positional contacts
|
|
r0 = pContacts[i]->pt[pContacts[i]->flags>>contact_bidx_log2&1]-body0->pos;
|
|
body0->Fcollision += pContacts[i]->dP*pContacts[i]->dPn; body0->Tcollision += r0^pContacts[i]->dP*pContacts[i]->dPn;
|
|
}
|
|
for(i=0;i<nAngContacts;i++) // angular contacts
|
|
pContacts[i]->vrel = body0->Iinv*body0->Tcollision;
|
|
for(;i<nContacts;i++) { // positional contacts
|
|
r0 = pContacts[i]->pt[pContacts[i]->flags>>contact_bidx_log2&1]-body0->pos;
|
|
pContacts[i]->vrel = body0->Fcollision*body0->Minv + (body0->Iinv*body0->Tcollision^r0);
|
|
}
|
|
for(i=0,pAp=0;i<nContacts;i++) {
|
|
pContacts[i]->vrel.x = pContacts[i]->dP*pContacts[i]->vrel;
|
|
pAp += sqr(pContacts[i]->vrel.x)*pContacts[i]->Kinv(0,0);
|
|
}
|
|
a = min(20.0,r2/max((real)1E-10,pAp));
|
|
for(i=0;i<nContacts;i++) {
|
|
pContacts[i]->r.x -= pContacts[i]->vrel.x*a;
|
|
pContacts[i]->P.x += pContacts[i]->dPn*a;
|
|
}
|
|
r2new = ComputeRc(body0,pContacts,nAngContacts,nContacts);
|
|
b = r2new/r2; r2=r2new;
|
|
for(i=0;i<nContacts;i++)
|
|
(pContacts[i]->dPn*=b) += pContacts[i]->r.x*pContacts[i]->Kinv(0,0);
|
|
} while(--iter && r2>sqr(pss->minSeparationSpeed));
|
|
|
|
// store velocities in v_unproj,w_unproj
|
|
body0->Fcollision.zero(); body0->Tcollision.zero();
|
|
for(i=0;i<nAngContacts;i++) // angular contacts
|
|
body0->Tcollision += pContacts[i]->dP*pContacts[i]->P.x;
|
|
for(;i<nContacts;i++) { // positional contacts
|
|
r0 = pContacts[i]->pt[pContacts[i]->flags>>contact_bidx_log2&1]-body0->pos;
|
|
body0->Fcollision += pContacts[i]->dP*pContacts[i]->P.x; body0->Tcollision += r0^pContacts[i]->dP*pContacts[i]->P.x;
|
|
}
|
|
g_infos[body0->bProcessed].w_unproj = body0->Iinv*body0->Tcollision;
|
|
g_infos[body0->bProcessed].v_unproj = body0->Fcollision*body0->Minv;
|
|
|
|
#ifdef _DEBUG
|
|
for(i=0,r2=r2new=0;i<nAngContacts;i++) {
|
|
r2 += sqr(pContacts[i]->r0.x);
|
|
r2new += sqr(g_infos[body0->bProcessed].w_unproj*pContacts[i]->dP-pContacts[i]->r0.x);
|
|
} for(;i<nContacts;i++) {
|
|
iop = pContacts[i]->flags>>contact_bidx_log2;
|
|
r2 += sqr(pContacts[i]->r0.x);
|
|
r2new += sqr((g_infos[body0->bProcessed].v_unproj+(g_infos[body0->bProcessed].w_unproj^pContacts[i]->pt[iop]-body0->pos))*
|
|
pContacts[i]->dP-pContacts[i]->r0.x);
|
|
}
|
|
iop = 0;
|
|
iop = 1;
|
|
#endif
|
|
float r2max,v2max,v2unproj;
|
|
vmax = 2.0f;
|
|
if (nAngContacts==nContacts)
|
|
r2max = 1;
|
|
else for(i=nAngContacts,r2max=0;i<nContacts;i++) {
|
|
r2max = max(r2max,(pContacts[i]->pt[pContacts[i]->flags>>contact_bidx_log2]-body0->pos).len2());
|
|
vmax = 1.0f;
|
|
}
|
|
for(i=0,v2max=0;i<nAngContacts;i++)
|
|
v2max = max(v2max,sqr(pContacts[i]->r0.x)*r2max);
|
|
for(;i<nContacts;i++)
|
|
v2max = max(v2max,sqr(pContacts[i]->r0.x));
|
|
v2max = min(v2max,sqr(pss->maxvUnproj));
|
|
v2unproj = max(g_infos[body0->bProcessed].v_unproj.len2(), g_infos[body0->bProcessed].w_unproj.len2()*r2max);
|
|
if (v2unproj > v2max*vmax) {
|
|
v2unproj = sqrt_tpl(v2max/v2unproj);
|
|
g_infos[body0->bProcessed].v_unproj *= v2unproj;
|
|
g_infos[body0->bProcessed].w_unproj *= v2unproj;
|
|
}
|
|
}
|
|
}
|
|
|
|
// update bodies' positions with v_unproj*dt,w_unproj*dt
|
|
for(i=0; i<nBodies; i++) {
|
|
j = g_pBodies[i]->bProcessed;
|
|
g_pBodies[i]->bProcessed = 0;
|
|
vectorf L = g_pBodies[i]->q*(g_pBodies[i]->Ibody*(g_infos[j].w_unproj*g_pBodies[i]->q));
|
|
g_pBodies[i]->Eunproj = (g_infos[j].v_unproj.len2()+(g_infos[j].w_unproj*L)*g_pBodies[i]->Minv)*0.5f;
|
|
if (g_pBodies[i]->Eunproj>0) {
|
|
g_pBodies[i]->pos += g_infos[j].v_unproj*time_interval;
|
|
if (g_infos[j].w_unproj.len2()*sqr(time_interval)<sqr(0.003f))
|
|
g_pBodies[i]->q += quaternionf(0,g_infos[j].w_unproj*0.5f)*g_pBodies[i]->q*time_interval;
|
|
else {
|
|
float wlen = g_infos[j].w_unproj.len();
|
|
//q = quaternionf(wlen*dt,w/wlen)*q;
|
|
g_pBodies[i]->q = GetRotationAA(wlen*time_interval,g_infos[j].w_unproj/wlen)*g_pBodies[i]->q;
|
|
}
|
|
g_pBodies[i]->q.Normalize();
|
|
// don't unpdate Iinv and w, no1 will appreciate it after this point
|
|
}
|
|
g_pBodies[i]->Fcollision = g_infos[j].Fcollision;
|
|
g_pBodies[i]->Tcollision = g_infos[j].Tcollision;
|
|
}
|
|
}//"LCPCG-unproj"
|
|
}
|
|
}
|
|
|
|
|
|
for(i=0;i<nBodies;i++) {
|
|
g_pBodies[i]->Fcollision = (g_pBodies[i]->P-g_pBodies[i]->Fcollision)*rtime_interval;
|
|
g_pBodies[i]->Tcollision = (g_pBodies[i]->L-g_pBodies[i]->Tcollision)*rtime_interval;
|
|
}
|
|
}
|