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; } }