void RendererDirectionalLight::setDirection(const PxVec3 &dir)
{
RENDERER_ASSERT(dir.magnitudeSquared() >= 0.1f, "Trying to give Direction Light invalid Direction value.");
if(dir.magnitudeSquared() >= 0.1f)
{
m_direction = dir;
m_direction.normalize();
}
}
bool Actor::patternFracture(const PxVec3& hitLocation, const PxVec3& dirIn, float scale, float vel, float radiusIn)
{
int compoundNr = -1;
int convexNr = -1;
PxVec3 normal;
float dist;
bool ret = false;
PxVec3 dir = dirIn;
mScene->getScene()->lockWrite();
bool hit = false;
if (dir.magnitudeSquared() < 0.5f)
{
dir = PxVec3(1.f,0.f,0.f);
hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
if(!hit)
{
dir = PxVec3(0.f,1.f,0.f);
hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
if(!hit)
{
dir = PxVec3(0.f,0.f,1.f);
hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
}
}
}
else
{
hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
}
if (hit)
{
float radius = mMinRadius + mRadiusMultiplier*radiusIn;
float impulseMagn = scale*vel*mImpulseScale;
if (mSheetFracture)
{
normal = ((Compound*)mCompounds[(uint32_t)compoundNr])->mNormal;
}
PxVec3 a(0.f,0.f,1.f);
normal.normalize();
a -= a.dot(normal)*normal;
if( a.magnitudeSquared() < 0.1f )
{
a = PxVec3(0.f,1.f,0.f);
a -= a.dot(normal)*normal;
}
a.normalize();
PxVec3 b(normal.cross(a));
PxMat33 trans(a,b,normal);
ret = base::Actor::patternFracture(hitLocation,dir,compoundNr,trans,radius,impulseMagn,impulseMagn);
}
mScene->getScene()->unlockWrite();
mRenderResourcesDirty = true;
return ret;
}
// Each frame, we do a shape query for static and dynamic objects
// If this is the first time the synchronize has been called, then we create
// a trigger actor with two spheres in the primary scene. This trigger
// actor is used to detect when objects move in and outside of the static and dynamic
// mirror range specified.
void MirrorScene::synchronizePrimaryScene(const physx::PxVec3 &cameraPos)
{
PxVec3 diff = cameraPos - mLastCameraLocation;
PxF32 dist = diff.magnitudeSquared();
if ( dist > mMirrorDistanceThreshold )
{
mLastCameraLocation = cameraPos;
if ( mTriggerActor == NULL )
{
createTriggerActor(cameraPos); // Create the scene mirroring trigger actor
}
if ( mTriggerActor )
{
mPrimaryScene.lockWrite(__FILE__,__LINE__);
mTriggerActor->setKinematicTarget( PxTransform(cameraPos) ); // Update the position of the trigger actor to be the current camera location
mPrimaryScene.unlockWrite();
}
}
// Now, iterate on all of the current actors which are being mirrored
// Only the primary scene after modifies this hash, so it is safe to do this
// without any concerns of thread locking.
// The mirrored scene thread does access the contents of this hash (MirrorActor)
{
mPrimaryScene.lockRead(__FILE__,__LINE__);
for (ActorHash::Iterator i=mActors.getIterator(); !i.done(); ++i)
{
MirrorActor *ma = i->second;
ma->synchronizePose(); // check to see if the position of this object in the primary
// scene has changed. If it has, then we create a command for the mirror scene to update
// it's mirror actor to that new position.
}
mPrimaryScene.unlockRead();
}
}
void integrateTransform(const PxTransform& curTrans, const PxVec3& linvel, const PxVec3& angvel, PxReal timeStep, PxTransform& result)
{
result.p = curTrans.p + linvel * timeStep;
//from void PxsDynamicsContext::integrateAtomPose(PxsRigidBody* atom, Cm::BitMap &shapeChangedMap) const:
// Integrate the rotation using closed form quaternion integrator
PxReal w = angvel.magnitudeSquared();
if(w != 0.0f)
{
w = PxSqrt(w);
if (w != 0.0f)
{
const PxReal v = timeStep * w * 0.5f;
const PxReal q = PxCos(v);
const PxReal s = PxSin(v) / w;
const PxVec3 pqr = angvel * s;
const PxQuat quatVel(pqr.x, pqr.y, pqr.z, 0);
PxQuat out; //need to have temporary, otherwise we may overwrite input if &curTrans == &result.
out = quatVel * curTrans.q;
out.x += curTrans.q.x * q;
out.y += curTrans.q.y * q;
out.z += curTrans.q.z * q;
out.w += curTrans.q.w * q;
result.q = out;
return;
}
}
//orientation stays the same - convert from quat to matrix:
result.q = curTrans.q;
}
void SampleVehicleWayPoints::update(const PxTransform& playerTransform, const PxF32 timestep)
{
//Increment the elapsed time
mTimeElapsed+=timestep;
//Work out the point on the crossing line of the next way-point that is closest to the player.
const PxTransform& nextWayPoint=mWayPoints[mProgress+1];
const PxVec3 v=nextWayPoint.p;
const PxVec3 w=nextWayPoint.q.getBasisVector0();
const PxVec3 p=playerTransform.p;
const PxVec3 pv=p-v;
const PxF32 t=pv.dot(w);
//Test if the player's position is inside the width of the line crossing the next way-point.
if(PxAbs(t) < LINEWIDTH)
{
//Now test if the shortest distance to the next crossing line is smaller than a threshold.
const PxVec3 linePos=v+w*t;
const PxVec3 diff=p-linePos;
const PxF32 dist2=diff.magnitudeSquared();
if(dist2<LINEDISTANCE2)
{
mProgress++;
}
}
if(mProgress == mNumWayPoints-1)
{
mMinTimeElapsed=PxMin(mTimeElapsed, mMinTimeElapsed);
mTimeElapsed=0;
mProgress=0;
}
}
bool Gu::intersectSphereSphere(const Gu::Sphere& sphere0, const Gu::Sphere& sphere1)
{
const PxVec3 delta = sphere1.center - sphere0.center;
const PxReal distanceSq = delta.magnitudeSquared();
const PxReal radSum = sphere0.radius + sphere1.radius;
return distanceSq <= radSum * radSum; // PT: objects are defined as closed, so we return 'true' in case of equality
}
bool Gu::intersectSphereBox(const Gu::Sphere& sphere, const Gu::Box& box)
{
const PxVec3 delta = sphere.center - box.center;
PxVec3 dRot = box.rot.transformTranspose(delta); //transform delta into OBB body coords. (use method call!)
//check if delta is outside ABB - and clip the vector to the ABB.
bool outside = false;
if(dRot.x < -box.extents.x)
{
outside = true;
dRot.x = -box.extents.x;
}
else if(dRot.x > box.extents.x)
{
outside = true;
dRot.x = box.extents.x;
}
if(dRot.y < -box.extents.y)
{
outside = true;
dRot.y = -box.extents.y;
}
else if(dRot.y > box.extents.y)
{
outside = true;
dRot.y = box.extents.y;
}
if(dRot.z < -box.extents.z)
{
outside = true;
dRot.z = -box.extents.z;
}
else if(dRot.z > box.extents.z)
{
outside = true;
dRot.z = box.extents.z;
}
if(outside) //if clipping was done, sphere center is outside of box.
{
const PxVec3 clippedDelta = box.rot.transform(dRot); //get clipped delta back in world coords.
const PxVec3 clippedVec = delta - clippedDelta; //what we clipped away.
const PxReal lenSquared = clippedVec.magnitudeSquared();
const PxReal radius = sphere.radius;
if(lenSquared > radius * radius) // PT: objects are defined as closed, so we return 'true' in case of equality
return false; //disjoint
}
return true;
}
static bool GeomOverlapCallback_SphereSphere(GEOM_OVERLAP_CALLBACK_PARAMS)
{
PX_ASSERT(geom0.getType()==PxGeometryType::eSPHERE);
PX_ASSERT(geom1.getType()==PxGeometryType::eSPHERE);
PX_UNUSED(cache);
const PxSphereGeometry& sphereGeom0 = static_cast<const PxSphereGeometry&>(geom0);
const PxSphereGeometry& sphereGeom1 = static_cast<const PxSphereGeometry&>(geom1);
const PxVec3 delta = transform1.p - transform0.p;
return delta.magnitudeSquared() <= Ps::sqr(sphereGeom0.radius + sphereGeom1.radius); // PT: objects are defined as closed, so we return 'true' in case of equality
}
bool physx::Gu::computeSphere_SphereMTD(const Sphere& sphere0, const Sphere& sphere1, PxSweepHit& hit)
{
const PxVec3 delta = sphere1.center - sphere0.center;
const PxReal d2 = delta.magnitudeSquared();
const PxReal radiusSum = sphere0.radius + sphere1.radius;
const PxReal d = PxSqrt(d2);
hit.normal = delta / d;
hit.distance = d - radiusSum ;
hit.position = sphere0.center + hit.normal * sphere0.radius;
return true;
}
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
}
bool physx::Gu::computeSphere_CapsuleMTD( const Sphere& sphere, const Capsule& capsule, PxSweepHit& hit)
{
const PxReal radiusSum = sphere.radius + capsule.radius;
PxReal u;
distancePointSegmentSquared(capsule, sphere.center, &u);
const PxVec3 normal = capsule.getPointAt(u) - sphere.center;
const PxReal lenSq = normal.magnitudeSquared();
const PxF32 d = PxSqrt(lenSq);
hit.normal = normal / d;
hit.distance = d - radiusSum;
hit.position = sphere.center + hit.normal * sphere.radius;
return true;
}
bool physx::Gu::computeCapsule_CapsuleMTD(const Capsule& capsule0, const Capsule& capsule1, PxSweepHit& hit)
{
using namespace Ps::aos;
PxReal s,t;
distanceSegmentSegmentSquared2(capsule0, capsule1, &s, &t);
const PxReal radiusSum = capsule0.radius + capsule1.radius;
const PxVec3 pointAtCapsule0 = capsule0.getPointAt(s);
const PxVec3 pointAtCapsule1 = capsule1.getPointAt(t);
const PxVec3 normal = pointAtCapsule0 - pointAtCapsule1;
const PxReal lenSq = normal.magnitudeSquared();
const PxF32 len = PxSqrt(lenSq);
hit.normal = normal / len;
hit.distance = len - radiusSum;
hit.position = pointAtCapsule1 + hit.normal * capsule1.radius;
return true;
}
void ClothImpl<SwCloth>::setVirtualParticles(Range<const uint32_t[4]> indices, Range<const PxVec3> weights)
{
mCloth.mNumVirtualParticles = 0;
// shuffle indices to form independent SIMD sets
uint16_t numParticles = uint16_t(mCloth.mCurParticles.size());
TripletScheduler scheduler(indices);
scheduler.simd(numParticles, 4);
// convert indices to byte offset
Vec4us dummy(numParticles, uint16_t(numParticles+1), uint16_t(numParticles+2), 0);
Vector<uint32_t>::Type::ConstIterator sIt = scheduler.mSetSizes.begin();
Vector<uint32_t>::Type::ConstIterator sEnd = scheduler.mSetSizes.end();
TripletScheduler::ConstTripletIter tIt = scheduler.mTriplets.begin(), tLast;
mCloth.mVirtualParticleIndices.resize(0);
mCloth.mVirtualParticleIndices.reserve(indices.size() + 3 * uint32_t(sEnd - sIt));
for(; sIt != sEnd; ++sIt)
{
uint32_t setSize = *sIt;
for(tLast = tIt + setSize; tIt != tLast; ++tIt, ++mCloth.mNumVirtualParticles)
mCloth.mVirtualParticleIndices.pushBack(Vec4us(*tIt));
mCloth.mVirtualParticleIndices.resize(
(mCloth.mVirtualParticleIndices.size() + 3) & ~3, dummy);
}
Vector<Vec4us>::Type(mCloth.mVirtualParticleIndices.begin(),
mCloth.mVirtualParticleIndices.end()).swap(mCloth.mVirtualParticleIndices);
// precompute 1/dot(w,w)
Vec4fAlignedVector().swap(mCloth.mVirtualParticleWeights);
mCloth.mVirtualParticleWeights.reserve(weights.size());
for(; !weights.empty(); weights.popFront())
{
PxVec3 w = reinterpret_cast<const PxVec3&>(weights.front());
PxReal scale = 1 / w.magnitudeSquared();
mCloth.mVirtualParticleWeights.pushBack(PxVec4(w.x, w.y, w.z, scale));
}
mCloth.notifyChanged();
}
void ApplyInverseSquareGravity(PxRigidActor* actor, PxVec3 source, PxReal power)
{
PxVec3 dir;
PxReal distSquared;
PxVec3 norm;
PxVec3 force;
int objectNum = boxes.size();
for(int i = 0; i < objectNum; i++)
{
//Disables the scene gravity so we can apply our own
DisableGravity(boxes[i]->actor);
dir = source - boxes[i]->actor->getGlobalPose().p;
distSquared = dir.magnitudeSquared();
distSquared = (distSquared < 10) ? 10000 : distSquared;
norm = dir.getNormalized();
force = (norm * power) / distSquared;
boxes[i]->actor->isRigidBody()->addForce(force, PxForceMode::eACCELERATION);
}
}
PxReal Sc::BodySim::updateWakeCounter(PxReal dt, PxReal energyThreshold, PxReal freezeThreshold, PxReal invDt, bool enableStabilization)
{
// update the body's sleep state and
BodyCore& core = getBodyCore();
PxReal wakeCounterResetTime = ScInternalWakeCounterResetValue;
PxReal wc = core.getWakeCounter();
{
if(enableStabilization)
{
bool isFrozen = false;
const PxTransform& body2World = getBody2World();
// calculate normalized energy: kinetic energy divided by mass
const PxVec3 t = core.getInverseInertia();
const PxVec3 inertia(t.x > 0.f ? 1.0f/t.x : 1.f, t.y > 0.f ? 1.0f/t.y : 1.f, t.z > 0.f ? 1.0f/t.z : 1.f);
PxVec3 sleepLinVelAcc = mLLBody.mAcceleration.linear;
PxVec3 sleepAngVelAcc = body2World.q.rotateInv(mLLBody.mAcceleration.angular);
// scale threshold by cluster factor (more contacts => higher sleep threshold)
//const PxReal clusterFactor = PxReal(1u + getNumUniqueInteractions());
const PxU32 clusterFactor = getNumUniqueInteractions();
PxReal invMass = core.getInverseMass();
if(invMass == 0.f)
invMass = 1.f;
const PxReal angular = sleepAngVelAcc.multiply(sleepAngVelAcc).dot(inertia) * invMass;
const PxReal linear = sleepLinVelAcc.magnitudeSquared();
PxReal frameNormalizedEnergy = 0.5f * (angular + linear);
const PxReal cf = readInternalFlag(BF_HAS_STATIC_TOUCH) && clusterFactor > 1 ? clusterFactor : 0.f;
const PxReal freezeThresh = cf*freezeThreshold;
mFreezeCount = PxMax(mFreezeCount-dt, 0.0f);
bool settled = true;
if (frameNormalizedEnergy >= freezeThresh)
{
settled = false;
mFreezeCount = PX_FREEZE_INTERVAL;
if(frameNormalizedEnergy >= (freezeThresh * cf))
{
mAccelScale = 0.f;
}
}
if(settled || mAccelScale > 0.f)
{
//Dampen bodies that are just about to go to sleep
const PxReal sleepDamping = PX_SLEEP_DAMPING;
const PxReal sleepDampingTimesDT=sleepDamping*dt;
const PxReal d=1.0f-sleepDampingTimesDT;
core.setLinearVelocity(core.getLinearVelocity()*d);
core.setAngularVelocity(core.getAngularVelocity()*d);
mAccelScale = invDt * PX_FREEZE_SCALE;
isFrozen = mFreezeCount == 0.f && frameNormalizedEnergy < freezeThreshold;
}
if(isFrozen)
{
getBodyCore().getCore().mInternalFlags |= PxsRigidCore::eFROZEN;
core.getCore().body2World = mLLBody.getLastCCDTransform();
}
else
getBodyCore().getCore().mInternalFlags &= (~PxsRigidCore::eFROZEN);
/*KS: New algorithm for sleeping when using stabilization:
* Energy *this frame* must be higher than sleep threshold and accumulated energy over previous frames
* must be higher than clusterFactor*energyThreshold.
*/
if(wc < wakeCounterResetTime * 0.5f || wc < dt)
{
//Accumulate energy
mSleepLinVelAcc += sleepLinVelAcc;
mSleepAngVelAcc += sleepAngVelAcc;
//If energy this frame is high
if (frameNormalizedEnergy >= energyThreshold)
{
//Compute energy over sleep preparation time
const PxReal sleepAngular = mSleepAngVelAcc.multiply(mSleepAngVelAcc).dot(inertia) * invMass;
const PxReal sleepLinear = mSleepLinVelAcc.magnitudeSquared();
PxReal normalizedEnergy = 0.5f * (sleepAngular + sleepLinear);
PxReal sleepClusterFactor = clusterFactor+1.f;
// scale threshold by cluster factor (more contacts => higher sleep threshold)
const PxReal threshold = sleepClusterFactor*energyThreshold;
//If energy over sleep preparation time is high
if(normalizedEnergy >= threshold)
{
//Wake up
PX_ASSERT(isActive());
resetSleepFilter();
const float factor = energyThreshold == 0.f ? 2.0f : PxMin(normalizedEnergy/threshold, 2.0f);
PxReal oldWc = wc;
wc = factor * 0.5f * wakeCounterResetTime + dt * (sleepClusterFactor - 1.0f);
core.setWakeCounterFromSim(wc);
if (oldWc == 0.0f) // for the case where a sleeping body got activated by the system (not the user) AND got processed by the solver as well
notifyNotReadyForSleeping();
return wc;
}
}
}
}
else if(wc < wakeCounterResetTime * 0.5f || wc < dt)
{
const PxTransform& body2World = getBody2World();
// calculate normalized energy: kinetic energy divided by mass
const PxVec3 t = core.getInverseInertia();
const PxVec3 inertia(t.x > 0.f ? 1.0f/t.x : 1.f, t.y > 0.f ? 1.0f/t.y : 1.f, t.z > 0.f ? 1.0f/t.z : 1.f);
PxVec3 sleepLinVelAcc = mLLBody.mAcceleration.linear;
PxVec3 sleepAngVelAcc = body2World.q.rotateInv(mLLBody.mAcceleration.angular);
mSleepLinVelAcc += sleepLinVelAcc;
mSleepAngVelAcc += sleepAngVelAcc;
PxReal invMass = core.getInverseMass();
if(invMass == 0.f)
invMass = 1.f;
const PxReal angular = mSleepAngVelAcc.multiply(mSleepAngVelAcc).dot(inertia) * invMass;
const PxReal linear = mSleepLinVelAcc.magnitudeSquared();
PxReal normalizedEnergy = 0.5f * (angular + linear);
// scale threshold by cluster factor (more contacts => higher sleep threshold)
const PxReal clusterFactor = PxReal(1 + getNumCountedInteractions());
const PxReal threshold = clusterFactor*energyThreshold;
if (normalizedEnergy >= threshold)
{
PX_ASSERT(isActive());
resetSleepFilter();
const float factor = threshold == 0.f ? 2.0f : PxMin(normalizedEnergy/threshold, 2.0f);
PxReal oldWc = wc;
wc = factor * 0.5f * wakeCounterResetTime + dt * (clusterFactor - 1.0f);
core.setWakeCounterFromSim(wc);
if (oldWc == 0.0f) // for the case where a sleeping body got activated by the system (not the user) AND got processed by the solver as well
notifyNotReadyForSleeping();
return wc;
}
}
}
wc = PxMax(wc-dt, 0.0f);
core.setWakeCounterFromSim(wc);
return wc;
}
static void tessellateTriangle(TessParams* tp, const PxVec3& v0, const PxVec3& v1, const PxVec3& v2)
{
tp->nbTessellation++;
if(!Gu::intersectTriangleBox(tp->cullingBoxCenter, tp->cullingBoxExtents, v0, v1, v2))
return;
PxU32 code;
{
const PxVec3 edge0 = v0 - v1;
const PxVec3 edge1 = v1 - v2;
const PxVec3 edge2 = v2 - v0;
const float maxEdgeLength2 = tp->maxEdgeLength2;
const bool split0 = edge0.magnitudeSquared()>maxEdgeLength2;
const bool split1 = edge1.magnitudeSquared()>maxEdgeLength2;
const bool split2 = edge2.magnitudeSquared()>maxEdgeLength2;
code = (PxU32(split2)<<2)|(PxU32(split1)<<1)|PxU32(split0);
}
const PxVec3 m0 = (v0 + v1)*0.5f;
const PxVec3 m1 = (v1 + v2)*0.5f;
const PxVec3 m2 = (v2 + v0)*0.5f;
switch(code)
{
case 0: // 000: no split
{
tp->worldTriangles->pushBack(PxTriangle(v0, v1, v2));
tp->triIndicesArray->pushBack(tp->index);
tp->nbNewTris++;
}
break;
case 1: // 001: split edge0
{
tessellateTriangle(tp, v0, m0, v2);
tessellateTriangle(tp, m0, v1, v2);
}
break;
case 2: // 010: split edge1
{
tessellateTriangle(tp, v0, v1, m1);
tessellateTriangle(tp, v0, m1, v2);
}
break;
case 3: // 011: split edge0/edge1
{
tessellateTriangle(tp, v0, m0, m1);
tessellateTriangle(tp, v0, m1, v2);
tessellateTriangle(tp, m0, v1, m1);
}
break;
case 4: // 100: split edge2
{
tessellateTriangle(tp, v0, v1, m2);
tessellateTriangle(tp, v1, v2, m2);
}
break;
case 5: // 101: split edge0/edge2
{
tessellateTriangle(tp, v0, m0, m2);
tessellateTriangle(tp, m0, v1, m2);
tessellateTriangle(tp, m2, v1, v2);
}
break;
case 6: // 110: split edge1/edge2
{
tessellateTriangle(tp, v0, v1, m1);
tessellateTriangle(tp, v0, m1, m2);
tessellateTriangle(tp, m2, m1, v2);
}
break;
case 7: // 111: split edge0/edge1/edge2
{
tessellateTriangle(tp, v0, m0, m2);
tessellateTriangle(tp, m0, v1, m1);
tessellateTriangle(tp, m2, m1, v2);
tessellateTriangle(tp, m0, m1, m2);
}
break;
};
}
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;
}
}
}
}
PxReal Gu::distancePointTriangleSquared( const PxVec3& point,
const PxVec3& triangleOrigin,
const PxVec3& triangleEdge0,
const PxVec3& triangleEdge1,
PxReal* param0,
PxReal* param1)
{
const PxVec3 kDiff = triangleOrigin - point;
const PxReal fA00 = triangleEdge0.magnitudeSquared();
const PxReal fA01 = triangleEdge0.dot(triangleEdge1);
const PxReal fA11 = triangleEdge1.magnitudeSquared();
const PxReal fB0 = kDiff.dot(triangleEdge0);
const PxReal fB1 = kDiff.dot(triangleEdge1);
const PxReal fC = kDiff.magnitudeSquared();
const PxReal fDet = PxAbs(fA00*fA11 - fA01*fA01);
PxReal fS = fA01*fB1-fA11*fB0;
PxReal fT = fA01*fB0-fA00*fB1;
PxReal fSqrDist;
if(fS + fT <= fDet)
{
if(fS < 0.0f)
{
if(fT < 0.0f) // region 4
{
if(fB0 < 0.0f)
{
fT = 0.0f;
if(-fB0 >= fA00)
{
fS = 1.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = -fB0/fA00;
fSqrDist = fB0*fS+fC;
}
}
else
{
fS = 0.0f;
if(fB1 >= 0.0f)
{
fT = 0.0f;
fSqrDist = fC;
}
else if(-fB1 >= fA11)
{
fT = 1.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fT = -fB1/fA11;
fSqrDist = fB1*fT+fC;
}
}
}
else // region 3
{
fS = 0.0f;
if(fB1 >= 0.0f)
{
fT = 0.0f;
fSqrDist = fC;
}
else if(-fB1 >= fA11)
{
fT = 1.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fT = -fB1/fA11;
fSqrDist = fB1*fT+fC;
}
}
}
else if(fT < 0.0f) // region 5
{
fT = 0.0f;
if(fB0 >= 0.0f)
{
fS = 0.0f;
fSqrDist = fC;
}
else if(-fB0 >= fA00)
{
fS = 1.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = -fB0/fA00;
fSqrDist = fB0*fS+fC;
}
}
else // region 0
{
// minimum at interior PxVec3
if(fDet==0.0f)
{
fS = 0.0f;
fT = 0.0f;
fSqrDist = PX_MAX_REAL;
}
else
{
PxReal fInvDet = 1.0f/fDet;
fS *= fInvDet;
fT *= fInvDet;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
}
else
{
PxReal fTmp0, fTmp1, fNumer, fDenom;
if(fS < 0.0f) // region 2
{
fTmp0 = fA01 + fB0;
fTmp1 = fA11 + fB1;
if(fTmp1 > fTmp0)
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
fS = 1.0f;
fT = 0.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = fNumer/fDenom;
fT = 1.0f - fS;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
else
{
fS = 0.0f;
if(fTmp1 <= 0.0f)
{
fT = 1.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else if(fB1 >= 0.0f)
{
fT = 0.0f;
fSqrDist = fC;
}
else
{
fT = -fB1/fA11;
fSqrDist = fB1*fT+fC;
}
}
}
else if(fT < 0.0f) // region 6
{
fTmp0 = fA01 + fB1;
fTmp1 = fA00 + fB0;
if(fTmp1 > fTmp0)
{
fNumer = fTmp1 - fTmp0;
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
fT = 1.0f;
fS = 0.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fT = fNumer/fDenom;
fS = 1.0f - fT;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
else
{
fT = 0.0f;
if(fTmp1 <= 0.0f)
{
fS = 1.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else if(fB0 >= 0.0f)
{
fS = 0.0f;
fSqrDist = fC;
}
else
{
fS = -fB0/fA00;
fSqrDist = fB0*fS+fC;
}
}
}
else // region 1
{
fNumer = fA11 + fB1 - fA01 - fB0;
if(fNumer <= 0.0f)
{
fS = 0.0f;
fT = 1.0f;
fSqrDist = fA11+2.0f*fB1+fC;
}
else
{
fDenom = fA00-2.0f*fA01+fA11;
if(fNumer >= fDenom)
{
fS = 1.0f;
fT = 0.0f;
fSqrDist = fA00+2.0f*fB0+fC;
}
else
{
fS = fNumer/fDenom;
fT = 1.0f - fS;
fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
}
}
}
}
if(param0) *param0 = fS;
if(param1) *param1 = fT;
// account for numerical round-off error
return physx::intrinsics::selectMax(0.0f, fSqrDist);
}
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;
}
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;
}
// this code is shared between capsule vertices and sphere
bool GuContactSphereHeightFieldShared(GU_CONTACT_METHOD_ARGS, bool isCapsule)
{
#if 1
PX_UNUSED(cache);
PX_UNUSED(renderOutput);
// Get actual shape data
const PxSphereGeometry& shapeSphere = shape0.get<const PxSphereGeometry>();
const PxHeightFieldGeometryLL& hfGeom = shape1.get<const PxHeightFieldGeometryLL>();
const Gu::HeightField& hf = *static_cast<Gu::HeightField*>(hfGeom.heightField);
const Gu::HeightFieldUtil hfUtil(hfGeom, hf);
const PxReal radius = shapeSphere.radius;
const PxReal eps = PxReal(0.1) * radius;
const PxVec3 sphereInHfShape = transform1.transformInv(transform0.p);
PX_ASSERT(isCapsule || contactBuffer.count==0);
const PxReal oneOverRowScale = hfUtil.getOneOverRowScale();
const PxReal oneOverColumnScale = hfUtil.getOneOverColumnScale();
// check if the sphere is below the HF surface
if (hfUtil.isShapePointOnHeightField(sphereInHfShape.x, sphereInHfShape.z))
{
PxReal fracX, fracZ;
const PxU32 vertexIndex = hfUtil.getHeightField().computeCellCoordinates(sphereInHfShape.x * oneOverRowScale, sphereInHfShape.z * oneOverColumnScale, fracX, fracZ);
// The sphere origin projects within the bounds of the heightfield in the X-Z plane
// const PxReal sampleHeight = hfShape.getHeightAtShapePoint(sphereInHfShape.x, sphereInHfShape.z);
const PxReal sampleHeight = hfUtil.getHeightAtShapePoint2(vertexIndex, fracX, fracZ);
const PxReal deltaHeight = sphereInHfShape.y - sampleHeight;
//if (hfShape.isDeltaHeightInsideExtent(deltaHeight, eps))
if (hf.isDeltaHeightInsideExtent(deltaHeight, eps))
{
// The sphere origin is 'below' the heightfield surface
// Actually there is an epsilon involved to make sure the
// 'above' surface calculations can deliver a good normal
const PxU32 faceIndex = hfUtil.getFaceIndexAtShapePointNoTest2(vertexIndex, fracX, fracZ);
if (faceIndex != 0xffffffff)
{
//hfShape.getAbsPoseFast().M.getColumn(1, hfShapeUp);
const PxVec3 hfShapeUp = transform1.q.getBasisVector1();
if (hf.getThicknessFast() <= 0)
contactBuffer.contact(transform0.p, hfShapeUp, deltaHeight-radius, faceIndex);
else
contactBuffer.contact(transform0.p, -hfShapeUp, -deltaHeight-radius, faceIndex);
return true;
}
return false;
}
}
const PxReal epsSqr = eps * eps;
const PxReal inflatedRadius = radius + params.mContactDistance;
const PxReal inflatedRadiusSquared = inflatedRadius * inflatedRadius;
const PxVec3 sphereInHF = hfUtil.shape2hfp(sphereInHfShape);
const PxReal radiusOverRowScale = inflatedRadius * PxAbs(oneOverRowScale);
const PxReal radiusOverColumnScale = inflatedRadius * PxAbs(oneOverColumnScale);
const PxU32 minRow = hf.getMinRow(sphereInHF.x - radiusOverRowScale);
const PxU32 maxRow = hf.getMaxRow(sphereInHF.x + radiusOverRowScale);
const PxU32 minColumn = hf.getMinColumn(sphereInHF.z - radiusOverColumnScale);
const PxU32 maxColumn = hf.getMaxColumn(sphereInHF.z + radiusOverColumnScale);
// this assert is here because the following code depends on it for reasonable performance for high-count situations
PX_COMPILE_TIME_ASSERT(ContactBuffer::MAX_CONTACTS == 64);
const PxU32 nbColumns = hf.getNbColumnsFast();
#define HFU Gu::HeightFieldUtil
PxU32 numFaceContacts = 0;
for (PxU32 i = 0; i<2; i++)
{
const bool facesOnly = (i == 0);
// first we go over faces-only meaning only contacts directly in Voronoi regions of faces
// at second pass we consider edges and vertices and clamp the normals to adjacent feature's normal
// if there was a prior contact. it is equivalent to clipping the normal to it's feature's Voronoi region
for (PxU32 r = minRow; r < maxRow; r++)
{
for (PxU32 c = minColumn; c < maxColumn; c++)
{
// x--x--x
// | x |
// x x x
// | x |
// x--x--x
PxVec3 closestPoints[11];
PxU32 closestFeatures[11];
PxU32 npcp = hfUtil.findClosestPointsOnCell(
r, c, sphereInHfShape, closestPoints, closestFeatures, facesOnly, !facesOnly, true);
for(PxU32 pi = 0; pi < npcp; pi++)
{
PX_ASSERT(closestFeatures[pi] != 0xffffffff);
const PxVec3 d = sphereInHfShape - closestPoints[pi];
if (hf.isDeltaHeightOppositeExtent(d.y)) // See if we are 'above' the heightfield
{
const PxReal dMagSq = d.magnitudeSquared();
if (dMagSq > inflatedRadiusSquared)
// Too far above
continue;
PxReal dMag = -1.0f; // dMag is sqrt(sMadSq) and comes up as a byproduct of other calculations in computePointNormal
PxVec3 n; // n is in world space, rotated by transform1
PxU32 featureType = HFU::getFeatureType(closestFeatures[pi]);
if (featureType == HFU::eEDGE)
{
PxU32 edgeIndex = HFU::getFeatureIndex(closestFeatures[pi]);
PxU32 adjFaceIndices[2];
const PxU32 adjFaceCount = hf.getEdgeTriangleIndices(edgeIndex, adjFaceIndices);
PxVec3 origin;
PxVec3 direction;
const PxU32 vertexIndex = edgeIndex / 3;
const PxU32 row = vertexIndex / nbColumns;
const PxU32 col = vertexIndex % nbColumns;
hfUtil.getEdge(edgeIndex, vertexIndex, row, col, origin, direction);
n = hfUtil.computePointNormal(
hfGeom.heightFieldFlags, d, transform1, dMagSq,
closestPoints[pi].x, closestPoints[pi].z, epsSqr, dMag);
PxVec3 localN = transform1.rotateInv(n);
// clamp the edge's normal to its Voronoi region
for (PxU32 j = 0; j < adjFaceCount; j++)
{
const PxVec3 adjNormal = hfUtil.hf2shapen(hf.getTriangleNormalInternal(adjFaceIndices[j])).getNormalized();
PxU32 triCell = adjFaceIndices[j] >> 1;
PxU32 triRow = triCell/hf.getNbColumnsFast();
PxU32 triCol = triCell%hf.getNbColumnsFast();
PxVec3 tv0, tv1, tv2, tvc;
hf.getTriangleVertices(adjFaceIndices[j], triRow, triCol, tv0, tv1, tv2);
tvc = hfUtil.hf2shapep((tv0+tv1+tv2)/3.0f); // compute adjacent triangle center
PxVec3 perp = adjNormal.cross(direction).getNormalized(); // adj face normal cross edge dir
if (perp.dot(tvc-origin) < 0.0f) // make sure perp is pointing toward the center of the triangle
perp = -perp;
// perp is now a vector sticking out of the edge of the triangle (also the test edge) pointing toward the center
// perpendicular to the normal (in triangle plane)
if (perp.dot(localN) > 0.0f) // if the normal is in perp halfspace, clamp it to Voronoi region
{
n = transform1.rotate(adjNormal);
break;
}
}
} else if(featureType == HFU::eVERTEX)
{
// AP: these contacts are rare so hopefully it's ok
const PxU32 bufferCount = contactBuffer.count;
const PxU32 vertIndex = HFU::getFeatureIndex(closestFeatures[pi]);
EdgeData adjEdges[8];
const PxU32 row = vertIndex / nbColumns;
const PxU32 col = vertIndex % nbColumns;
const PxU32 numAdjEdges = ::getVertexEdgeIndices(hf, vertIndex, row, col, adjEdges);
for (PxU32 iPrevEdgeContact = numFaceContacts; iPrevEdgeContact < bufferCount; iPrevEdgeContact++)
{
if (contactBuffer.contacts[iPrevEdgeContact].forInternalUse != HFU::eEDGE)
continue; // skip non-edge contacts (can be other vertex contacts)
for (PxU32 iAdjEdge = 0; iAdjEdge < numAdjEdges; iAdjEdge++)
// does adjacent edge index for this vertex match a previously encountered edge index?
if (adjEdges[iAdjEdge].edgeIndex == contactBuffer.contacts[iPrevEdgeContact].internalFaceIndex1)
{
// if so, clamp the normal for this vertex to that edge's normal
n = contactBuffer.contacts[iPrevEdgeContact].normal;
dMag = PxSqrt(dMagSq);
break;
}
}
}
if (dMag == -1.0f)
n = hfUtil.computePointNormal(hfGeom.heightFieldFlags, d, transform1,
dMagSq, closestPoints[pi].x, closestPoints[pi].z, epsSqr, dMag);
PxVec3 p = transform0.p - n * radius;
#if DEBUG_RENDER_HFCONTACTS
printf("n=%.5f %.5f %.5f; ", n.x, n.y, n.z);
if (n.y < 0.8f)
int a = 1;
PxScene *s; PxGetPhysics().getScenes(&s, 1, 0);
Cm::RenderOutput((Cm::RenderBuffer&)s->getRenderBuffer()) << Cm::RenderOutput::LINES << PxDebugColor::eARGB_BLUE // red
<< p << (p + n * 10.0f);
#endif
// temporarily use the internalFaceIndex0 slot in the contact buffer for featureType
contactBuffer.contact(
p, n, dMag - radius, PxU16(featureType), HFU::getFeatureIndex(closestFeatures[pi]));
}
}
}
}
