Transform trMul(const Transform& trA, const Transform& trB)
{
Transform ans;
ans.m_rotation = mtMul( trA.m_rotation, trB.m_rotation );
ans.m_translation = mtMul1( trA.m_rotation, trB.m_translation ) + trA.m_translation;
return ans;
}
Transform trInvert( const Transform& tr )
{
Transform ans;
ans.m_rotation = mtTranspose( tr.m_rotation );
ans.m_translation = mtMul1( ans.m_rotation, -tr.m_translation );
return ans;
}
void calcRigidMotion(const float4* vtx, const float4* vtxPrev, int nVtx, float4& v, float4& w, float4& c)
{
float4 ang;
Matrix3x3 inertia;
c = make_float4(0);
v = make_float4(0);
ang = make_float4(0);
inertia = mtIdentity();
for(int i=0; i<nVtx; i++)
{
c += vtx[i];
v += vtx[i]-vtxPrev[i];
}
c /= (float)nVtx;
v /= (float)nVtx;
for(int i=0; i<nVtx; i++)
{
const float4& r = vtx[i]-c;
ang += cross3( r, vtx[i]-vtxPrev[i] );
Matrix3x3 m;
m.m_row[0] = make_float4(r.y*r.y + r.z*r.z, -r.x*r.y, -r.x*r.z);
m.m_row[1] = make_float4(-r.y*r.x, r.x*r.x + r.z*r.z, -r.y*r.z);
m.m_row[2] = make_float4(-r.z*r.x, -r.z*r.y, r.x*r.x + r.y*r.y);
inertia = inertia + m;
}
Matrix3x3 invInertia = mtInvert( inertia );
w = mtMul1( invInertia, ang );
}
float4 trMul1(const Transform& tr, const float4& p)
{
return mtMul1( tr.m_rotation, p ) + tr.m_translation;
}
static
__inline
void solveFriction(Constraint4& cs,
const float4& posA, float4& linVelA, float4& angVelA, float invMassA, const Matrix3x3& invInertiaA,
const float4& posB, float4& linVelB, float4& angVelB, float invMassB, const Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
const float4& center = cs.m_center;
float4 n = -cs.m_linear;
float4 tangent[2];
#if 1
btPlaneSpace1 (&n, &tangent[0],&tangent[1]);
#else
float4 r = cs.m_worldPos[0]-center;
tangent[0] = cross3( n, r );
tangent[1] = cross3( tangent[0], n );
tangent[0] = normalize3( tangent[0] );
tangent[1] = normalize3( tangent[1] );
#endif
float4 angular0, angular1, linear;
float4 r0 = center - posA;
float4 r1 = center - posB;
for(int i=0; i<2; i++)
{
setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB );
rambdaDt *= cs.m_fJacCoeffInv[i];
{
float prevSum = cs.m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = max2( updated, minRambdaDt[i] );
updated = min2( updated, maxRambdaDt[i] );
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
float4 linImp0 = invMassA*linear*rambdaDt;
float4 linImp1 = invMassB*(-linear)*rambdaDt;
float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x));
btAssert(_finite(linImp1.x));
#endif
linVelA += linImp0;
angVelA += angImp0;
linVelB += linImp1;
angVelB += angImp1;
}
{ // angular damping for point constraint
float4 ab = normalize3( posB - posA );
float4 ac = normalize3( center - posA );
if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{
float angNA = dot3F4( n, angVelA );
float angNB = dot3F4( n, angVelB );
angVelA -= (angNA*0.1f)*n;
angVelB -= (angNB*0.1f)*n;
}
}
}
static
__inline
void solveContact(Constraint4& cs,
const float4& posA, float4& linVelA, float4& angVelA, float invMassA, const Matrix3x3& invInertiaA,
const float4& posB, float4& linVelB, float4& angVelB, float invMassB, const Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
float4 dLinVelA = make_float4(0.f);
float4 dAngVelA = make_float4(0.f);
float4 dLinVelB = make_float4(0.f);
float4 dAngVelB = make_float4(0.f);
for(int ic=0; ic<4; ic++)
{
// dont necessary because this makes change to 0
if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
{
float4 angular0, angular1, linear;
float4 r0 = cs.m_worldPos[ic] - posA;
float4 r1 = cs.m_worldPos[ic] - posB;
setLinearAndAngular( -cs.m_linear, r0, r1, linear, angular0, angular1 );
float rambdaDt = calcRelVel(cs.m_linear, -cs.m_linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
rambdaDt *= cs.m_jacCoeffInv[ic];
{
float prevSum = cs.m_appliedRambdaDt[ic];
float updated = prevSum;
updated += rambdaDt;
updated = max2( updated, minRambdaDt[ic] );
updated = min2( updated, maxRambdaDt[ic] );
rambdaDt = updated - prevSum;
cs.m_appliedRambdaDt[ic] = updated;
}
float4 linImp0 = invMassA*linear*rambdaDt;
float4 linImp1 = invMassB*(-linear)*rambdaDt;
float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x));
btAssert(_finite(linImp1.x));
#endif
if( JACOBI )
{
dLinVelA += linImp0;
dAngVelA += angImp0;
dLinVelB += linImp1;
dAngVelB += angImp1;
}
else
{
linVelA += linImp0;
angVelA += angImp0;
linVelB += linImp1;
angVelB += angImp1;
}
}
}
if( JACOBI )
{
linVelA += dLinVelA;
angVelA += dAngVelA;
linVelB += dLinVelB;
angVelB += dAngVelB;
}
}
