PX_INLINE void computeFrictionTangents(const PxVec3& vrel,const PxVec3& unitNormal, PxVec3& t0, PxVec3& t1)
{
PX_ASSERT(PxAbs(unitNormal.magnitude()-1)<1e-3f);
t0 = vrel - unitNormal * unitNormal.dot(vrel);
PxReal ll = t0.magnitudeSquared();
if (ll > 0.1f) //can set as low as 0.
{
t0 *= PxRecipSqrt(ll);
t1 = unitNormal.cross(t0);
}
else
Ps::normalToTangents(unitNormal, t0, t1); //fallback
}
/**
\brief computes a oriented bounding box around the scaled basis.
\param basis Input = skewed basis, Output = (normalized) orthogonal basis.
\return Bounding box extent.
*/
PxVec3 optimizeBoundingBox(PxMat33& basis)
{
PxVec3* PX_RESTRICT vec = &basis[0]; // PT: don't copy vectors if not needed...
// PT: since we store the magnitudes to memory, we can avoid the FCMPs afterwards
PxVec3 magnitude( vec[0].magnitudeSquared(), vec[1].magnitudeSquared(), vec[2].magnitudeSquared() );
// find indices sorted by magnitude
#ifdef PX_X360
int i = (PxU32&)(magnitude[1]) > (PxU32&)(magnitude[ 0 ]) ? 1 : 0;
int j = (PxU32&)(magnitude[2]) > (PxU32&)(magnitude[1-i]) ? 2 : 1-i;
#else
int i = magnitude[1] > magnitude[ 0 ] ? 1 : 0;
int j = magnitude[2] > magnitude[1-i] ? 2 : 1-i;
#endif
const int k = 3 - i - j;
#ifdef PX_X360
if((PxU32&)(magnitude[i]) < (PxU32&)(magnitude[j]))
#else
if(magnitude[i] < magnitude[j])
#endif
swap(i, j);
// ortho-normalize basis
PxReal invSqrt = PxRecipSqrt(magnitude[i]);
magnitude[i] *= invSqrt; vec[i] *= invSqrt; // normalize the first axis
PxReal dotij = vec[i].dot(vec[j]);
PxReal dotik = vec[i].dot(vec[k]);
magnitude[i] += PxAbs(dotij) + PxAbs(dotik); // elongate the axis by projection of the other two
vec[j] -= vec[i] * dotij; // orthogonize the two remaining axii relative to vec[i]
vec[k] -= vec[i] * dotik;
magnitude[j] = vec[j].normalize();
PxReal dotjk = vec[j].dot(vec[k]);
magnitude[j] += PxAbs(dotjk); // elongate the axis by projection of the other one
vec[k] -= vec[j] * dotjk; // orthogonize vec[k] relative to vec[j]
magnitude[k] = vec[k].normalize();
return magnitude;
}
PX_FORCE_INLINE void collideWithSphere(PxsParticleCollData& collData, const PxSphereGeometry& sphereShapeData,
PxReal proxRadius)
{
PxVec3& oldPos = collData.localOldPos;
PxVec3& newPos = collData.localNewPos;
PxReal radius = sphereShapeData.radius;
PxReal oldPosDist2 = oldPos.magnitudeSquared();
PxReal radius2 = radius * radius;
bool oldInSphere = (oldPosDist2 < radius2);
if(oldInSphere)
{
// old position inside the skeleton
// add ccd with time 0.0
collData.localSurfaceNormal = oldPos;
if (oldPosDist2 > 0.0f)
collData.localSurfaceNormal *= PxRecipSqrt(oldPosDist2);
else
collData.localSurfaceNormal = PxVec3(0,1.0f,0);
// Push particle to surface such that the distance to the surface is equal to the collision radius
collData.localSurfacePos = collData.localSurfaceNormal * (radius + collData.restOffset);
collData.ccTime = 0.0;
collData.localFlags |= PXS_FLUID_COLL_FLAG_L_CC;
}
else
{
// old position is outside of the skeleton
PxVec3 motion = newPos - oldPos;
// Discriminant
PxReal b = motion.dot(oldPos) * 2.0f;
PxReal a2 = 2.0f * motion.magnitudeSquared();
PxReal disc = (b*b) - (2.0f * a2 * (oldPosDist2 - radius2));
bool intersection = disc > 0.0f;
if ((!intersection) || (a2 == 0.0f))
{
// the ray does not intersect the sphere
collideWithSphereNonContinuous(collData, newPos, radius, proxRadius);
}
else
{
// the ray intersects the sphere
PxReal t = -(b + PxSqrt(disc)) / a2; // Compute intersection point
if (t < 0.0f || t > 1.0f)
{
// intersection point lies outside motion vector
collideWithSphereNonContinuous(collData, newPos, radius, proxRadius);
}
else if(t < collData.ccTime)
{
// intersection point lies on sphere, add lcc
//collData.localSurfacePos = oldPos + (motion * t);
//collData.localSurfaceNormal = collData.localSurfacePos;
//collData.localSurfaceNormal *= (1.0f / radius);
//collData.localSurfacePos += (collData.localSurfaceNormal * collData.restOffset);
PxVec3 relativeImpact = motion*t;
collData.localSurfaceNormal = oldPos + relativeImpact;
collData.localSurfaceNormal *= (1.0f / radius);
computeContinuousTargetPosition(collData.localSurfacePos, collData.localOldPos, relativeImpact, collData.localSurfaceNormal, collData.restOffset);
collData.ccTime = t;
collData.localFlags |= PXS_FLUID_COLL_FLAG_L_CC;
}
}
}
}
PX_FORCE_INLINE PxU32 collideWithMeshTriangle(PxVec3& surfaceNormal, PxVec3& surfacePos,
PxVec3& proxSurfaceNormal, PxVec3& proxSurfacePos,
PxReal& ccTime, PxReal& distOldToSurface,
const PxVec3& oldPos, const PxVec3& newPos,
const PxVec3& origin, const PxVec3& e0,
const PxVec3& e1, bool hasCC,
const PxReal& collRadius, const PxReal& proxRadius)
{
PxU32 flags = 0;
PxReal collisionRadius2 = collRadius * collRadius;
PxReal proximityRadius2 = proxRadius * proxRadius;
PxVec3 motion = newPos - oldPos;
// dc and proximity tests
PxVec3 tmpV = origin - newPos;
PxReal a = e0.dot(e0);
PxReal b = e0.dot(e1);
PxReal c = e1.dot(e1);
PxReal d = e0.dot(tmpV);
PxReal e = e1.dot(tmpV);
PxVec3 coords;
coords.x = b*e - c*d; // s * det
coords.y = b*d - a*e; // t * det
coords.z = a*c - b*b; // det
bool insideCase = false;
PxVec3 clampedCoords(PxVec3(0));
if (coords.x <= 0.0f)
{
c = PxMax(c, FLT_MIN);
clampedCoords.y = -e/c;
}
else if (coords.y <= 0.0f)
{
a = PxMax(a, FLT_MIN);
clampedCoords.x = -d/a;
}
else if (coords.x + coords.y > coords.z)
{
PxReal denominator = a + c - b - b;
PxReal numerator = c + e - b - d;
denominator = PxMax(denominator, FLT_MIN);
clampedCoords.x = numerator / denominator;
clampedCoords.y = 1.0f - clampedCoords.x;
}
else // all inside
{
PxReal tmpF = PxMax(coords.z, FLT_MIN);
tmpF = 1.0f / tmpF;
clampedCoords.x = coords.x * tmpF;
clampedCoords.y = coords.y * tmpF;
insideCase = true;
}
clampedCoords.x = PxMax(clampedCoords.x, 0.0f);
clampedCoords.y = PxMax(clampedCoords.y, 0.0f);
clampedCoords.x = PxMin(clampedCoords.x, 1.0f);
clampedCoords.y = PxMin(clampedCoords.y, 1.0f);
// Closest point to particle inside triangle
PxVec3 closest = origin + e0 * clampedCoords.x + e1 * clampedCoords.y;
PxVec3 triangleOffset = newPos - closest;
PxReal triangleDistance2 = triangleOffset.magnitudeSquared();
PxVec3 triangleNormal = e0.cross(e1);
PxReal e0e1Span = triangleNormal.magnitude();
bool isInFront = triangleOffset.dot(triangleNormal) > 0.0f;
// MS: Possible optimzation
/*
if (isInFront && (triangleDistance2 >= proximityRadius2))
return flags;
*/
bool isInProximity = insideCase && (triangleDistance2 < proximityRadius2) && isInFront;
bool isInDiscrete = (triangleDistance2 < collisionRadius2) && isInFront;
if (!hasCC)
{
// Only apply discrete and proximity collisions if no continuous collisions was detected so far (for any colliding shape)
if (isInDiscrete)
{
if (triangleDistance2 > PXS_FLUID_COLL_TRI_DISTANCE)
{
surfaceNormal = triangleOffset * PxRecipSqrt(triangleDistance2);
}
else
{
surfaceNormal = triangleNormal * (1.0f / e0e1Span);
}
surfacePos = closest + (surfaceNormal * collRadius);
flags |= PXS_FLUID_COLL_FLAG_L_DC;
}
if (isInProximity)
{
proxSurfaceNormal = triangleNormal * (1.0f / e0e1Span);
proxSurfacePos = closest + (proxSurfaceNormal * collRadius);
flags |= PXS_FLUID_COLL_FLAG_L_PROX;
tmpV = (oldPos - origin); //this time it's not the newPosition offset.
distOldToSurface = proxSurfaceNormal.dot(tmpV); // Need to return the distance to decide which constraints should be thrown away
}
}
if (!isInDiscrete && !isInProximity)
{
// cc test (let's try only executing this if no discrete coll, or proximity happend).
tmpV = origin - oldPos; //this time it's not the newPosition offset.
PxReal pDistN = triangleNormal.dot(tmpV);
PxReal rLengthN = triangleNormal.dot(motion);
if (pDistN > 0.0f || rLengthN >= pDistN)
return flags;
//we are in the half closed interval [0.0f, 1.0)
PxReal t = pDistN / rLengthN;
PX_ASSERT((t >= 0.0f) && (t < 1.0f));
PxVec3 relativePOSITION = (motion * t);
PxVec3 testPoint = oldPos + relativePOSITION;
// a,b,c and coords.z don't depend on test point -> still valid
tmpV = origin - testPoint;
d = e0.dot(tmpV);
e = e1.dot(tmpV);
coords.x = b*e - c*d;
coords.y = b*d - a*e;
//maybe we don't need this for rare case leaking on triangle boundaries?
PxReal eps = coords.z * PXS_FLUID_COLL_RAY_EPSILON_FACTOR;
if ((coords.x >= -eps) && (coords.y >= -eps) && (coords.x + coords.y <= coords.z + eps))
{
PxReal invLengthN = (1.0f / e0e1Span);
distOldToSurface = -pDistN * invLengthN; // Need to return the distance to decide which constraints should be thrown away
surfaceNormal = triangleNormal * invLengthN;
//surfacePos = testPoint + (surfaceNormal * collRadius);
computeContinuousTargetPosition(surfacePos, oldPos, relativePOSITION, surfaceNormal, collRadius);
ccTime = t;
flags |= PXS_FLUID_COLL_FLAG_L_CC;
}
}
return flags;
}
bool PxcContactCapsuleCapsule(CONTACT_METHOD_ARGS)
{
PX_UNUSED(npCache);
// Get actual shape data
const PxCapsuleGeometry& shapeCapsule0 = shape0.get<const PxCapsuleGeometry>();
const PxCapsuleGeometry& shapeCapsule1 = shape1.get<const PxCapsuleGeometry>();
// Capsule-capsule contact generation
Gu::Segment segment[2];
segment[0].p0 = transform0.q.getBasisVector0() * shapeCapsule0.halfHeight;
segment[0].p1 = -segment[0].p0;
segment[1].p0 = transform1.q.getBasisVector0() * shapeCapsule1.halfHeight;
segment[1].p1 = -segment[1].p0;
PxVec3 delta = transform1.p - transform0.p;
segment[1].p1 += delta;
segment[1].p0 += delta;
// Collision detection
PxReal s,t;
#ifdef USE_NEW_VERSION
PxReal squareDist = Gu::distanceSegmentSegmentSquared2(segment[0], segment[1], &s, &t);
#else
PxReal squareDist = PxcDistanceSegmentSegmentSquaredOLD(segment[0].point0, segment[0].direction(), segment[1].point0, segment[1].direction(), &s, &t);
#endif
const PxReal radiusSum = shapeCapsule0.radius + shapeCapsule1.radius;
const PxReal inflatedSum = radiusSum + contactDistance;
const PxReal inflatedSumSquared = inflatedSum*inflatedSum;
if(squareDist < inflatedSumSquared)
{
PxVec3 dir[2];
dir[0] = segment[0].computeDirection();
dir[1] = segment[1].computeDirection();
PxReal segLen[2];
segLen[0] = dir[0].magnitude();
segLen[1] = dir[1].magnitude();
if (segLen[0]) dir[0] *= 1.0f / segLen[0];
if (segLen[1]) dir[1] *= 1.0f / segLen[1];
if (PxAbs(dir[0].dot(dir[1])) > 0.9998f) //almost parallel, ca. 1 degree difference --> generate two contact points at ends
{
PxU32 numCons = 0;
PxReal segLenEps[2];
segLenEps[0] = segLen[0] * 0.001f;//0.1% error is ok.
segLenEps[1] = segLen[1] * 0.001f;
//project the two end points of each onto the axis of the other and take those 4 points.
//we could also generate a single normal at the single closest point, but this would be 'unstable'.
for (PxU32 destShapeIndex = 0; destShapeIndex < 2; destShapeIndex ++)
{
for (PxU32 startEnd = 0; startEnd < 2; startEnd ++)
{
const PxU32 srcShapeIndex = 1-destShapeIndex;
//project start/end of srcShapeIndex onto destShapeIndex.
PxVec3 pos[2];
pos[destShapeIndex] = startEnd ? segment[srcShapeIndex].p1 : segment[srcShapeIndex].p0;
const PxReal p = dir[destShapeIndex].dot(pos[destShapeIndex] - segment[destShapeIndex].p0);
if (p >= -segLenEps[destShapeIndex] && p <= (segLen[destShapeIndex] + segLenEps[destShapeIndex]))
{
pos[srcShapeIndex] = p * dir[destShapeIndex] + segment[destShapeIndex].p0;
PxVec3 normal = pos[1] - pos[0];
const PxReal normalLenSq = normal.magnitudeSquared();
if (normalLenSq > 1e-6 && normalLenSq < inflatedSumSquared)
{
const PxReal distance = PxSqrt(normalLenSq);
normal *= 1.0f/distance;
PxVec3 point = pos[1] - normal * (srcShapeIndex ? shapeCapsule1 : shapeCapsule0).radius;
point += transform0.p;
contactBuffer.contact(point, normal, distance - radiusSum);
numCons++;
}
}
}
}
if (numCons) //if we did not have contacts, then we may have the case where they are parallel, but are stacked end to end, in which case the old code will generate good contacts.
return true;
}
// Collision response
PxVec3 pos1 = segment[0].getPointAt(s);
PxVec3 pos2 = segment[1].getPointAt(t);
PxVec3 normal = pos1 - pos2;
const PxReal normalLenSq = normal.magnitudeSquared();
if (normalLenSq < 1e-6)
{
// Zero normal -> pick the direction of segment 0.
// Not always accurate but consistent with FW.
if (segLen[0] > 1e-6)
normal = dir[0];
else
normal = PxVec3(1.0f, 0.0f, 0.0f);
}
else
{
normal *= PxRecipSqrt(normalLenSq);
}
pos1 += transform0.p;
contactBuffer.contact(pos1 - normal * shapeCapsule0.radius, normal, PxSqrt(squareDist) - radiusSum);
return true;
}
return false;
}
