static __inline void solveContact(b3GpuConstraint4& cs,
const b3Vector3& posA, const b3Vector3& linVelARO, const b3Vector3& angVelARO, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, const b3Vector3& linVelBRO, const b3Vector3& angVelBRO, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4], b3Vector3& dLinVelA, b3Vector3& dAngVelA, b3Vector3& dLinVelB, b3Vector3& dAngVelB)
{
for(int ic=0; ic<4; ic++)
{
// dont necessary because this makes change to 0
if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
{
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
setLinearAndAngular( (const b3Vector3 &)cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 );
float rambdaDt = calcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,
linVelARO+dLinVelA, angVelARO+dAngVelA, linVelBRO+dLinVelB, angVelBRO+dAngVelB ) + cs.m_b[ic];
rambdaDt *= cs.m_jacCoeffInv[ic];
{
float prevSum = cs.m_appliedRambdaDt[ic];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max( updated, minRambdaDt[ic] );
updated = b3Min( updated, maxRambdaDt[ic] );
rambdaDt = updated - prevSum;
cs.m_appliedRambdaDt[ic] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp1.getX()));
#endif
if (invMassA)
{
dLinVelA += linImp0;
dAngVelA += angImp0;
}
if (invMassB)
{
dLinVelB += linImp1;
dAngVelB += angImp1;
}
}
}
}
void solveContact3(b3GpuConstraint4* cs,
b3Vector3* posAPtr, b3Vector3* linVelA, b3Vector3* angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
b3Vector3* posBPtr, b3Vector3* linVelB, b3Vector3* angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
b3Vector3* dLinVelA, b3Vector3* dAngVelA, b3Vector3* dLinVelB, b3Vector3* dAngVelB)
{
float minRambdaDt = 0;
float maxRambdaDt = FLT_MAX;
for(int ic=0; ic<4; ic++)
{
if( cs->m_jacCoeffInv[ic] == 0.f ) continue;
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = cs->m_worldPos[ic] - *posAPtr;
b3Vector3 r1 = cs->m_worldPos[ic] - *posBPtr;
setLinearAndAngular( cs->m_linear, r0, r1, linear, angular0, angular1 );
float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1,
*linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];
rambdaDt *= cs->m_jacCoeffInv[ic];
{
float prevSum = cs->m_appliedRambdaDt[ic];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max( updated, minRambdaDt );
updated = b3Min( updated, maxRambdaDt );
rambdaDt = updated - prevSum;
cs->m_appliedRambdaDt[ic] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
if (invMassA)
{
*dLinVelA += linImp0;
*dAngVelA += angImp0;
}
if (invMassB)
{
*dLinVelB += linImp1;
*dAngVelB += angImp1;
}
}
}
static
__inline
void solveFriction(b3GpuConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
const b3Vector3& center = (const b3Vector3&)cs.m_center;
b3Vector3 n = -(const b3Vector3&)cs.m_linear;
b3Vector3 tangent[2];
#if 1
b3PlaneSpace1 (n, tangent[0],tangent[1]);
#else
b3Vector3 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
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = center - posA;
b3Vector3 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 = b3Max( updated, minRambdaDt[i] );
updated = b3Min( updated, maxRambdaDt[i] );
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp1.getX()));
#endif
linVelA += linImp0;
angVelA += angImp0;
linVelB += linImp1;
angVelB += angImp1;
}
{ // angular damping for point constraint
b3Vector3 ab = ( posB - posA ).normalized();
b3Vector3 ac = ( center - posA ).normalized();
if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{
float angNA = b3Dot( n, angVelA );
float angNB = b3Dot( n, angVelB );
angVelA -= (angNA*0.1f)*n;
angVelB -= (angNB*0.1f)*n;
}
}
}
void setConstraint4( const b3Vector3& posA, const b3Vector3& linVelA, const b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, const b3Vector3& linVelB, const b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
b3Contact4* src, float dt, float positionDrift, float positionConstraintCoeff, float countA, float countB,
b3GpuConstraint4* dstC )
{
dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
float dtInv = 1.f/dt;
for(int ic=0; ic<4; ic++)
{
dstC->m_appliedRambdaDt[ic] = 0.f;
}
dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
dstC->m_linear = src->m_worldNormalOnB;
dstC->m_linear[3] = 0.7f ;//src->getFrictionCoeff() );
for(int ic=0; ic<4; ic++)
{
b3Vector3 r0 = src->m_worldPosB[ic] - posA;
b3Vector3 r1 = src->m_worldPosB[ic] - posB;
if( ic >= src->m_worldNormalOnB[3] )//npoints
{
dstC->m_jacCoeffInv[ic] = 0.f;
continue;
}
float relVelN;
{
b3Vector3 linear, angular0, angular1;
setLinearAndAngular(src->m_worldNormalOnB, r0, r1, linear, angular0, angular1);
dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
invMassA, &invInertiaA, invMassB, &invInertiaB ,countA,countB);
relVelN = calcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB);
float e = 0.f;//src->getRestituitionCoeff();
if( relVelN*relVelN < 0.004f )
{
e = 0.f;
}
dstC->m_b[ic] = e*relVelN;
//float penetration = src->m_worldPos[ic].w;
dstC->m_b[ic] += (src->m_worldPosB[ic][3] + positionDrift)*positionConstraintCoeff*dtInv;
dstC->m_appliedRambdaDt[ic] = 0.f;
}
}
if( src->m_worldNormalOnB[3] > 0 )//npoints
{ // prepare friction
b3Vector3 center = make_float4(0.f);
for(int i=0; i<src->m_worldNormalOnB[3]; i++)
center += src->m_worldPosB[i];
center /= (float)src->m_worldNormalOnB[3];
b3Vector3 tangent[2];
b3PlaneSpace1(src->m_worldNormalOnB,tangent[0],tangent[1]);
b3Vector3 r[2];
r[0] = center - posA;
r[1] = center - posB;
for(int i=0; i<2; i++)
{
b3Vector3 linear, angular0, angular1;
setLinearAndAngular(tangent[i], r[0], r[1], linear, angular0, angular1);
dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
invMassA, &invInertiaA, invMassB, &invInertiaB ,countA,countB);
dstC->m_fAppliedRambdaDt[i] = 0.f;
}
dstC->m_center = center;
}
for(int i=0; i<4; i++)
{
if( i<src->m_worldNormalOnB[3] )
{
dstC->m_worldPos[i] = src->m_worldPosB[i];
}
else
{
dstC->m_worldPos[i] = make_float4(0.f);
}
}
}
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;
}
}
