static
__forceinline
float calcRelVel(const float4& l0, const float4& l1, const float4& a0, const float4& a1,
const float4& linVel0, const float4& angVel0, const float4& linVel1, const float4& angVel1)
{
return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);
}
float ClothSimulation::calcVolume( const float4* v, const int4* t, int n )
{
float vol = 0;
for(int i=0; i<n; i++)
{
const int4& tri = t[i];
float4 c = cross3(v[tri.y], v[tri.x]);
vol += dot3F4( c, v[tri.z] );
}
return fabs( vol/6.f );
}
int collideStraight(const ConvexHeightField* shapeA,const ConvexHeightField* shapeB,
const float4& bodyApos, Quaternion& bodyAquat,const float4& bodyBpos,const Quaternion& bodyBquat,
ContactPoint4* contactsOut, int& numContacts, int contactCapacity,
float collisionMargin )
{
// Stopwatch sw;
Transform trA;
trA = trSetTransform(bodyApos,bodyAquat);
Transform trB;
trB = trSetTransform(bodyBpos, bodyBquat);
Transform B2A;
{
Transform invTrA = trInvert( trA );
B2A = trMul( invTrA, trB );
}
int nContacts = 0;
{ // testB against A
float4 p[ConvexHeightField::HEIGHT_RES*ConvexHeightField::HEIGHT_RES*6];
int nHits = 0;
const float4* pInB = shapeB->getSamplePoints();
float4 baInB = qtInvRotate( bodyBquat, bodyApos - bodyBpos );
if( shapeA->m_type == CollisionShape::SHAPE_HEIGHT_FIELD )
baInB = make_float4(0,0,0,0);
// sw.start();
for(int iface=0; iface<6; iface++)
{
Aabb aabb = shapeB->m_faceAabbs[iface];
aabb.transform( B2A.m_translation, B2A.m_rotation );
if( !shapeA->m_aabb.overlaps( aabb ) ) continue;
for(int ip=0; ip<ConvexHeightField::HEIGHT_RES*ConvexHeightField::HEIGHT_RES; ip++)
{
int i = iface*ConvexHeightField::HEIGHT_RES*ConvexHeightField::HEIGHT_RES+ip;
if( dot3F4( baInB, pInB[i] ) < 0.f ) continue;
float4 pInA = trMul1( B2A, pInB[i] );
if( shapeA->m_aabb.overlaps( pInA ) )
{
// Stopwatch sw1;
// sw1.start();
float dist = shapeA->queryDistance( pInA );
// sw1.stop();
// m_times[TIME_SAMPLE] += sw1.getMs();
if( dist < collisionMargin )
{
p[nHits] = make_float4(pInA.x, pInA.y, pInA.z, dist);
nHits++;
}
}
}
}
// sw.stop();
// m_times[TIME_TEST] += sw.getMs();
// sw.start();
if( nHits )
{
float4 ab = bodyBpos - bodyApos;
ab = qtInvRotate( bodyAquat, ab );
if( shapeA->m_type == CollisionShape::SHAPE_HEIGHT_FIELD )
{
//todo. sample normal from height field but just fake here
ab = make_float4(0,1,0,0);
}
int cIdx[4];
float4 center;
nContacts = extractManifold( p, nHits, ab, center, cIdx );
float4 contactNormal;
{
shapeA->queryDistanceWithNormal( center, contactNormal );
contactNormal = normalize3( contactNormal );
// u32 cmp = u8vCompress( contactNormal );
// contactNormal = make_float4( u8vGetX(cmp), u8vGetY(cmp), u8vGetZ(cmp), 0 );
}
int writeIdx = atomAdd( &numContacts, 1 );
if( writeIdx+1 < contactCapacity )
{
ContactPoint4& c = contactsOut[writeIdx];
nContacts = min2( nContacts, 4 );
for(int i=0; i<nContacts; i++)
{
c.m_worldPos[i] = transform( p[cIdx[i]], bodyApos, bodyAquat );
c.m_worldPos[i].w = max2( p[cIdx[i]].w - collisionMargin, -2*collisionMargin );
}
c.m_worldNormal = normalize3( qtRotate( bodyAquat, contactNormal ) );
c.m_restituitionCoeff = 0.f;
c.m_frictionCoeff = 0.7f;
//c.m_bodyAPtr = (void*)bodyAIdx;
//c.m_bodyBPtr = (void*)bodyBIdx;
c.getNPoints() = nContacts;
}
}
// sw.stop();
// m_times[TIME_MANIFOLD] += sw.getMs();
}
return nContacts;
}
int extractManifold(const float4* p, int nPoints, float4& nearNormal, float4& centerOut,
int contactIdx[4])
{
if( nPoints == 0 ) return 0;
nPoints = min2( nPoints, 64 );
float4 center = make_float4(0.f);
{
float4 v[64];
memcpy( v, p, nPoints*sizeof(float4) );
PARALLEL_SUM( v, nPoints );
center = v[0]/(float)nPoints;
}
centerOut = center;
{ // sample 4 directions
if( nPoints < 4 )
{
for(int i=0; i<nPoints; i++) contactIdx[i] = i;
return nPoints;
}
float4 aVector = p[0] - center;
float4 u = cross3( nearNormal, aVector );
float4 v = cross3( nearNormal, u );
u = normalize3( u );
v = normalize3( v );
int idx[4];
float2 max00 = make_float2(0,FLT_MAX);
{
float4 dir0 = u;
float4 dir1 = -u;
float4 dir2 = v;
float4 dir3 = -v;
// idx, distance
{
{
int4 a[64];
for(int ie = 0; ie<nPoints; ie++ )
{
float4 f;
float4 r = p[ie]-center;
f.x = dot3F4( dir0, r );
f.y = dot3F4( dir1, r );
f.z = dot3F4( dir2, r );
f.w = dot3F4( dir3, r );
a[ie].x = ((*(u32*)&f.x) & 0xffffff00);
a[ie].x |= (0xff & ie);
a[ie].y = ((*(u32*)&f.y) & 0xffffff00);
a[ie].y |= (0xff & ie);
a[ie].z = ((*(u32*)&f.z) & 0xffffff00);
a[ie].z |= (0xff & ie);
a[ie].w = ((*(u32*)&f.w) & 0xffffff00);
a[ie].w |= (0xff & ie);
}
for(int ie=0; ie<nPoints; ie++)
{
a[0].x = (a[0].x > a[ie].x )? a[0].x: a[ie].x;
a[0].y = (a[0].y > a[ie].y )? a[0].y: a[ie].y;
a[0].z = (a[0].z > a[ie].z )? a[0].z: a[ie].z;
a[0].w = (a[0].w > a[ie].w )? a[0].w: a[ie].w;
}
idx[0] = (int)a[0].x & 0xff;
idx[1] = (int)a[0].y & 0xff;
idx[2] = (int)a[0].z & 0xff;
idx[3] = (int)a[0].w & 0xff;
}
}
{
float2 h[64];
PARALLEL_DO( h[ie] = make_float2((float)ie, p[ie].w), nPoints );
REDUCE_MIN( h, nPoints );
max00 = h[0];
}
}
contactIdx[0] = idx[0];
contactIdx[1] = idx[1];
contactIdx[2] = idx[2];
contactIdx[3] = idx[3];
// if( max00.y < 0.0f )
// contactIdx[0] = (int)max00.x;
std::sort( contactIdx, contactIdx+4 );
return 4;
}
}
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;
}
}
}
