void RenderPhysX3Debug::addArrow(const PxVec3& posA, const PxVec3& posB, const RendererColor& color)
{
const PxVec3 t0 = (posB - posA).getNormalized();
const PxVec3 a = PxAbs(t0.x)<0.707f ? PxVec3(1,0,0): PxVec3(0,1,0);
const PxVec3 t1 = t0.cross(a).getNormalized();
const PxVec3 t2 = t0.cross(t1).getNormalized();
addLine(posA, posB, color);
addLine(posB, posB - t0*0.15 + t1 * 0.15, color);
addLine(posB, posB - t0*0.15 - t1 * 0.15, color);
addLine(posB, posB - t0*0.15 + t2 * 0.15, color);
addLine(posB, posB - t0*0.15 - t2 * 0.15, color);
}
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;
}
// -------------------------------------------------------------------------------------
void PolygonTriangulator::importPoints(const PxVec3 *points, int numPoints, const int *indices, PxVec3 *planeNormal, bool &isConvex)
{
// find projection 3d -> 2d;
PxVec3 n;
isConvex = true;
if (planeNormal)
n = *planeNormal;
else {
PX_ASSERT(numPoints >= 3);
n = PxVec3(0.0f, 0.0f, 0.0f);
for (int i = 1; i < numPoints-1; i++) {
int i0 = 0;
int i1 = i;
int i2 = i+1;
if (indices) {
i0 = indices[i0];
i1 = indices[i1];
i2 = indices[i2];
}
const PxVec3 &p0 = points[i0];
const PxVec3 &p1 = points[i1];
const PxVec3 &p2 = points[i2];
PxVec3 ni = (p1-p0).cross(p2-p0);
if (i > 1 && ni.dot(n) < 0.0f)
isConvex = false;
n += ni;
}
}
n.normalize();
PxVec3 t0,t1;
if (fabs(n.x) < fabs(n.y) && fabs(n.x) < fabs(n.z))
t0 = PxVec3(1.0f, 0.0f, 0.0f);
else if (fabs(n.y) < fabs(n.z))
t0 = PxVec3(0.0f, 1.0f, 0.0f);
else
t0 = PxVec3(0.0f, 0.0f, 1.0f);
t1 = n.cross(t0);
t1.normalize();
t0 = t1.cross(n);
mPoints.resize((uint32_t)numPoints);
if (indices == NULL) {
for (uint32_t i = 0; i < (uint32_t)numPoints; i++)
mPoints[i] = PxVec3(points[i].dot(t0), points[i].dot(t1), 0.0f);
}
else {
for (uint32_t i = 0; i < (uint32_t)numPoints; i++) {
const PxVec3 &p = points[(uint32_t)indices[i]];
mPoints[i] = PxVec3(p.dot(t0), p.dot(t1), 0.0f);
}
}
}
static PxQuat directionToQuaternion(const PxVec3& direction)
{
PxVec3 vUp(0.0f, 1.0f, 0.0f);
PxVec3 vRight = vUp.cross(direction);
vUp = direction.cross(vRight);
PxQuat qrot(PxMat33(vRight, vUp, direction));
qrot.normalize();
return qrot;
}
void SampleNorthPoleCameraController::update(Camera& camera, PxReal dtime)
{
// Update CCT
if(!mBase.isPaused())
{
PxVec3 targetKeyDisplacement(0);
PxVec3 targetPadDisplacement(0);
PxVec3 forward = camera.getViewDir();
forward.y = 0;
forward.normalize();
PxVec3 up = PxVec3(0,1,0);
PxVec3 right = forward.cross(up);
if(mFwd) targetKeyDisplacement += forward;
if(mBwd) targetKeyDisplacement -= forward;
if(mRight) targetKeyDisplacement += right;
if(mLeft) targetKeyDisplacement -= right;
targetKeyDisplacement *= mKeyShiftDown ? mRunningSpeed : mWalkingSpeed;
targetKeyDisplacement += PxVec3(0,-9.81,0);
targetKeyDisplacement *= dtime;
targetPadDisplacement += forward * mGamepadForwardInc * mRunningSpeed;
targetPadDisplacement += right * mGamepadLateralInc * mRunningSpeed;
targetPadDisplacement += PxVec3(0,-9.81,0);
targetPadDisplacement *= dtime;
PxU32 flags = mCCT.move(targetKeyDisplacement + targetPadDisplacement, 0.001f, dtime, PxControllerFilters(0));
PX_UNUSED(flags);
}
// Update camera
{
mTargetYaw += mGamepadYawInc * dtime;
mTargetPitch += mGamepadPitchInc * dtime;
// Clamp pitch
if(mTargetPitch<mPitchMin) mTargetPitch = mPitchMin;
if(mTargetPitch>mPitchMax) mTargetPitch = mPitchMax;
camera.setRot(PxVec3(-mTargetPitch,-mTargetYaw,0));
PxExtendedVec3 camTarget = computeCameraTarget();
const float filteredHeight = feedbackFilter((float)camTarget.y, mFilterMemory, dtime*6.0f);
camTarget.y = filteredHeight;
const PxF32 distanceToTarget = 0.0f;
const PxVec3 target = toVec3(camTarget) - camera.getViewDir()*distanceToTarget;
camera.setPos(target);
}
}
bool Gu::intersectEdgeEdge(const PxVec3& p1, const PxVec3& p2, const PxVec3& dir, const PxVec3& p3, const PxVec3& p4, PxReal& dist, PxVec3& ip)
{
const PxVec3 v1 = p2 - p1;
// Build plane P based on edge (p1, p2) and direction (dir)
Gu::Plane plane;
plane.normal = v1.cross(dir);
plane.d = -(plane.normal.dot(p1));
// if colliding edge (p3,p4) does not cross plane return no collision
// same as if p3 and p4 on same side of plane return 0
//
// Derivation:
// d3 = d(p3, P) = (p3 | plane.n) - plane.d; Reversed sign compared to Plane::Distance() because plane.d is negated.
// d4 = d(p4, P) = (p4 | plane.n) - plane.d; Reversed sign compared to Plane::Distance() because plane.d is negated.
// if d3 and d4 have the same sign, they're on the same side of the plane => no collision
// We test both sides at the same time by only testing Sign(d3 * d4).
// ### put that in the Plane class
// ### also check that code in the triangle class that might be similar
const PxReal d3 = plane.distance(p3);
PxReal temp = d3 * plane.distance(p4);
if(temp>0.0f) return false;
// if colliding edge (p3,p4) and plane are parallel return no collision
PxVec3 v2 = p4 - p3;
temp = plane.normal.dot(v2);
if(temp==0.0f) return false; // ### epsilon would be better
// compute intersection point of plane and colliding edge (p3,p4)
ip = p3-v2*(d3/temp);
// find largest 2D plane projection
PxU32 i,j;
Ps::closestAxis(plane.normal, i, j);
// compute distance of intersection from line (ip, -dir) to line (p1,p2)
dist = (v1[i]*(ip[j]-p1[j])-v1[j]*(ip[i]-p1[i]))/(v1[i]*dir[j]-v1[j]*dir[i]);
if(dist<0.0f) return false;
// compute intersection point on edge (p1,p2) line
ip -= dist*dir;
// check if intersection point (ip) is between edge (p1,p2) vertices
temp = (p1.x-ip.x)*(p2.x-ip.x)+(p1.y-ip.y)*(p2.y-ip.y)+(p1.z-ip.z)*(p2.z-ip.z);
if(temp<0.0f) return true; // collision found
return false; // no collision
}
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
}
void CookingAbstract::PhysicalMesh::computeTriangleAreas()
{
smallestTriangleArea = largestTriangleArea = 0.0f;
if (indices == NULL || vertices == NULL)
{
return;
}
smallestTriangleArea = PX_MAX_F32;
for (PxU32 i = 0; i < numIndices; i += 3)
{
const PxVec3 edge1 = vertices[indices[i + 1]] - vertices[indices[i]];
const PxVec3 edge2 = vertices[indices[i + 2]] - vertices[indices[i]];
const PxF32 triangleArea = edge1.cross(edge2).magnitude();
largestTriangleArea = PxMax(largestTriangleArea, triangleArea);
smallestTriangleArea = PxMin(smallestTriangleArea, triangleArea);
}
}
bool Character::faceToward(const PxVec3& targetDir, PxReal angleLimitPerFrame)
{
PxVec3 oldDir = mCharacterPose.q.rotate(PxVec3(0,0,1));
PxVec3 up(0,1,0);
PxVec3 newDir = PxVec3(targetDir.x, 0, targetDir.z).getNormalized();
PxVec3 right = -1.0f * oldDir.cross(up);
PxReal cos = newDir.dot(oldDir);
PxReal sin = newDir.dot(right);
PxReal angle = atan2(sin, cos);
PxReal limit = angleLimitPerFrame * (PxPi / 180.0f);
if (angle > limit) angle = limit;
else if (angle < -limit) angle = -limit;
PxQuat qdel(angle, up);
mCharacterPose.q = qdel * mCharacterPose.q;
return true;
}
bool Gu::intersectEdgeEdgeNEW(const PxVec3& p1, const PxVec3& p2, const PxVec3& dir, const PxVec3& p3, const PxVec3& p4, PxReal& dist, PxVec3& ip)
{
// Build plane P based on edge (p1, p2) and direction (dir)
const PxVec3 v12 = p2-p1;
Gu::Plane plane;
plane.normal = v12.cross(dir);
plane.d = -(plane.normal.dot(p1));
PxReal d3 = plane.distance(p3);
PxReal d4 = plane.distance(p4);
// line doesn't intersect plane if both have same sign or both are 0.
if(d3*d4>0 || d3==d4)
return false;
// vector from p1 to intersection point of plane and edge (p3,p4)
PxVec3 v1i = (d3*p4 - d4*p3)/(d3-d4) - p1;
// find largest 2D plane projection
PxU32 i,j;
Ps::closestAxis(plane.normal, i, j);
// compute distance of v1i to intersection of line (v1i, v1i-dir) and line (0,v12)
PxReal d = (v12[i]*v1i[j]-v12[j]*v1i[i])/(v12[i]*dir[j]-v12[j]*dir[i]);
if(d<0.0f)
return false;
// vector from p1 to intersection point of two lines above
v1i -= d*dir;
// we are allowed to write invalid output
dist = d;
ip = p1 + v1i;
// return if intersection point is on sweep side and between p1 and p2
return v1i.dot(v1i-v12)<0.0f;
}
bool setupFinalizeExtSolverContactsCoulomb(
const ContactBuffer& buffer,
const CorrelationBuffer& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
PxU8* workspace,
PxReal invDt,
PxReal bounceThresholdF32,
const SolverExtBody& b0,
const SolverExtBody& b1,
PxU32 frictionCountPerPoint,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1,
PxReal restDist,
PxReal ccdMaxDistance)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
const FloatV ccdMaxSeparation = FLoad(ccdMaxDistance);
PxU8* PX_RESTRICT ptr = workspace;
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b1.mLinkIndex];
const FloatV maxPenBias = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);
const FloatV restDistance = FLoad(restDist);
const FloatV bounceThreshold = FLoad(bounceThresholdF32);
const FloatV invDtV = FLoad(invDt);
const FloatV pt8 = FLoad(0.8f);
const FloatV invDtp8 = FMul(invDtV, pt8);
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
const PxU32 frictionPatchCount = c.frictionPatchCount;
const PxU32 pointStride = sizeof(SolverContactPointExt);
const PxU32 frictionStride = sizeof(SolverContactFrictionExt);
const PxU8 pointHeaderType = DY_SC_TYPE_EXT_CONTACT;
const PxU8 frictionHeaderType = DY_SC_TYPE_EXT_FRICTION;
PxReal d0 = invMassScale0;
PxReal d1 = invMassScale1;
PxReal angD0 = invInertiaScale0;
PxReal angD1 = invInertiaScale1;
PxU8 flags = 0;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const Vec3V normalV = Ps::aos::V3LoadA(contactBase0->normal);
const Vec3V normal = V3LoadA(contactBase0->normal);
const PxReal combinedRestitution = contactBase0->restitution;
SolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<SolverContactCoulombHeader*>(ptr);
ptr += sizeof(SolverContactCoulombHeader);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const FloatV restitution = FLoad(combinedRestitution);
header->numNormalConstr = PxU8(contactCount);
header->type = pointHeaderType;
//header->setRestitution(combinedRestitution);
header->setDominance0(d0);
header->setDominance1(d1);
header->angDom0 = angD0;
header->angDom1 = angD1;
header->flags = flags;
header->setNormal(normalV);
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
p += pointStride;
setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDtV, invDtp8, restDistance, maxPenBias, restitution,
bounceThreshold, contact, *solverContact, ccdMaxSeparation);
}
ptr = p;
}
}
//construct all the frictions
PxU8* PX_RESTRICT ptr2 = workspace;
const PxF32 orthoThreshold = 0.70710678f;
const PxF32 eps = 0.00001f;
bool hasFriction = false;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
SolverContactCoulombHeader* header = reinterpret_cast<SolverContactCoulombHeader*>(ptr2);
header->frictionOffset = PxU16(ptr - ptr2);
ptr2 += sizeof(SolverContactCoulombHeader) + header->numNormalConstr * pointStride;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
PxVec3 normal = contactBase0->normal;
const PxReal staticFriction = contactBase0->staticFriction;
const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
const bool haveFriction = (disableStrongFriction == 0);
SolverFrictionHeader* frictionHeader = reinterpret_cast<SolverFrictionHeader*>(ptr);
frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatchContactCounts[i] * frictionCountPerPoint : 0);
frictionHeader->flags = flags;
ptr += sizeof(SolverFrictionHeader);
PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
PxMemZero(forceBuffer, sizeof(PxF32) * c.frictionPatchContactCounts[i]);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
const PxReal sqDist = t0_.dot(t0_);
const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
const PxVec3 tDir1 = tDir0.cross(normal);
PxVec3 tFallback[2] = {tDir0, tDir1};
PxU32 ind = 0;
if(haveFriction)
{
hasFriction = true;
frictionHeader->setStaticFriction(staticFriction);
frictionHeader->invMass0D0 = d0;
frictionHeader->invMass1D1 = d1;
frictionHeader->angDom0 = angD0;
frictionHeader->angDom1 = angD1;
frictionHeader->type = frictionHeaderType;
PxU32 totalPatchContactCount = 0;
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const PxU32 start = c.contactPatches[patch].start;
const Gu::ContactPoint* contactBase = buffer.contacts + start;
PxU8* p = ptr;
for(PxU32 j =0; j < count; j++)
{
const Gu::ContactPoint& contact = contactBase[j];
const PxVec3 ra = contact.point - bodyFrame0.p;
const PxVec3 rb = contact.point - bodyFrame1.p;
const PxVec3 targetVel = contact.targetVel;
const PxVec3 pVRa = b0.getLinVel() + b0.getAngVel().cross(ra);
const PxVec3 pVRb = b1.getLinVel() + b1.getAngVel().cross(rb);
//const PxVec3 vrel = pVRa - pVRb;
for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
{
SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(p);
p += frictionStride;
PxVec3 t0 = tFallback[ind];
ind = 1 - ind;
PxVec3 raXn = ra.cross(t0);
PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t0, -rbXn, b1);
PxReal unitResponse = getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1);
PxReal tv = targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
tv += pVRa.dot(t0);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
tv -= pVRb.dot(t0);
f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
f0->setRaXn(resp0.angular);
f0->setRbXn(-resp1.angular);
f0->targetVel = tv;
f0->setNormal(t0);
f0->setAppliedForce(0.0f);
f0->linDeltaVA = V3LoadA(deltaV0.linear);
f0->angDeltaVA = V3LoadA(deltaV0.angular);
f0->linDeltaVB = V3LoadA(deltaV1.linear);
f0->angDeltaVB = V3LoadA(deltaV1.angular);
}
}
totalPatchContactCount += c.contactPatches[patch].count;
ptr = p;
}
}
}
//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
return hasFriction;
}
// 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]));
}
}
}
}
void setupFinalizeExtSolverContacts(
const ContactPoint* buffer,
const CorrelationBuffer& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
PxU8* workspace,
const SolverExtBody& b0,
const SolverExtBody& b1,
const PxReal invDtF32,
PxReal bounceThresholdF32,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1,
const PxReal restDist,
PxU8* frictionDataPtr,
PxReal ccdMaxContactDist)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
/*const bool haveFriction = PX_IR(n.staticFriction) > 0 || PX_IR(n.dynamicFriction) > 0;*/
const FloatV ccdMaxSeparation = FLoad(ccdMaxContactDist);
PxU8* PX_RESTRICT ptr = workspace;
const FloatV zero=FZero();
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b1.mLinkIndex];
const FloatV maxPenBias = FLoad(PxMax(maxPenBias0, maxPenBias1));
const PxReal d0 = invMassScale0;
const PxReal d1 = invMassScale1;
const PxReal angD0 = invInertiaScale0;
const PxReal angD1 = invInertiaScale1;
Vec4V staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W = V4Zero();
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetZ(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(d0));
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetW(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(d1));
const FloatV restDistance = FLoad(restDist);
PxU32 frictionPatchWritebackAddrIndex = 0;
PxU32 contactWritebackCount = 0;
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
const FloatV invDt = FLoad(invDtF32);
const FloatV p8 = FLoad(0.8f);
const FloatV bounceThreshold = FLoad(bounceThresholdF32);
const FloatV invDtp8 = FMul(invDt, p8);
PxU8 flags = 0;
for(PxU32 i=0;i<c.frictionPatchCount;i++)
{
PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const FrictionPatch& frictionPatch = c.frictionPatches[i];
PX_ASSERT(frictionPatch.anchorCount <= 2); //0==anchorCount is allowed if all the contacts in the manifold have a large offset.
const Gu::ContactPoint* contactBase0 = buffer + c.contactPatches[c.correlationListHeads[i]].start;
const PxReal combinedRestitution = contactBase0->restitution;
const PxReal staticFriction = contactBase0->staticFriction;
const PxReal dynamicFriction = contactBase0->dynamicFriction;
const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetX(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(staticFriction));
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetY(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(dynamicFriction));
SolverContactHeader* PX_RESTRICT header = reinterpret_cast<SolverContactHeader*>(ptr);
ptr += sizeof(SolverContactHeader);
Ps::prefetchLine(ptr + 128);
Ps::prefetchLine(ptr + 256);
Ps::prefetchLine(ptr + 384);
const bool haveFriction = (disableStrongFriction == 0) ;//PX_IR(n.staticFriction) > 0 || PX_IR(n.dynamicFriction) > 0;
header->numNormalConstr = Ps::to8(contactCount);
header->numFrictionConstr = Ps::to8(haveFriction ? frictionPatch.anchorCount*2 : 0);
header->type = Ps::to8(DY_SC_TYPE_EXT_CONTACT);
header->flags = flags;
const FloatV restitution = FLoad(combinedRestitution);
header->staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W = staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W;
header->angDom0 = angD0;
header->angDom1 = angD1;
const PxU32 pointStride = sizeof(SolverContactPointExt);
const PxU32 frictionStride = sizeof(SolverContactFrictionExt);
const Vec3V normal = V3LoadU(buffer[c.contactPatches[c.correlationListHeads[i]].start].normal);
header->normal = normal;
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
p += pointStride;
setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDt, invDtp8, restDistance, maxPenBias, restitution,
bounceThreshold, contact, *solverContact, ccdMaxSeparation);
}
ptr = p;
}
contactWritebackCount += contactCount;
PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
PxMemZero(forceBuffer, sizeof(PxF32) * contactCount);
ptr += sizeof(PxF32) * ((contactCount + 3) & (~3));
header->broken = 0;
if(haveFriction)
{
//const Vec3V normal = Vec3V_From_PxVec3(buffer.contacts[c.contactPatches[c.correlationListHeads[i]].start].normal);
PxVec3 normalS = buffer[c.contactPatches[c.correlationListHeads[i]].start].normal;
PxVec3 t0, t1;
computeFrictionTangents(b0.getLinVel() - b1.getLinVel(), normalS, t0, t1);
Vec3V vT0 = V3LoadU(t0);
Vec3V vT1 = V3LoadU(t1);
//We want to set the writeBack ptr to point to the broken flag of the friction patch.
//On spu we have a slight problem here because the friction patch array is
//in local store rather than in main memory. The good news is that the address of the friction
//patch array in main memory is stored in the work unit. These two addresses will be equal
//except on spu where one is local store memory and the other is the effective address in main memory.
//Using the value stored in the work unit guarantees that the main memory address is used on all platforms.
PxU8* PX_RESTRICT writeback = frictionDataPtr + frictionPatchWritebackAddrIndex*sizeof(FrictionPatch);
header->frictionBrokenWritebackByte = writeback;
for(PxU32 j = 0; j < frictionPatch.anchorCount; j++)
{
SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
ptr += frictionStride;
SolverContactFrictionExt* PX_RESTRICT f1 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
ptr += frictionStride;
PxVec3 ra = bodyFrame0.q.rotate(frictionPatch.body0Anchors[j]);
PxVec3 rb = bodyFrame1.q.rotate(frictionPatch.body1Anchors[j]);
PxVec3 error = (ra + bodyFrame0.p) - (rb + bodyFrame1.p);
{
const PxVec3 raXn = ra.cross(t0);
const PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);
FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1));
const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);
PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
PxF32 targetVel = buffer[index].targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel -= b0.projectVelocity(t0, raXn);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel += b1.projectVelocity(t0, rbXn);
f0->normalXYZ_appliedForceW = V4SetW(vT0, zero);
f0->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
f0->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t0.dot(error) * invDtF32));
f0->linDeltaVA = V3LoadA(deltaV0.linear);
f0->angDeltaVA = V3LoadA(deltaV0.angular);
f0->linDeltaVB = V3LoadA(deltaV1.linear);
f0->angDeltaVB = V3LoadA(deltaV1.angular);
f0->targetVel = targetVel;
}
{
const PxVec3 raXn = ra.cross(t1);
const PxVec3 rbXn = rb.cross(t1);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t1, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);
FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1));
const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);
PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
PxF32 targetVel = buffer[index].targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel -= b0.projectVelocity(t1, raXn);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel += b1.projectVelocity(t1, rbXn);
f1->normalXYZ_appliedForceW = V4SetW(vT1, zero);
f1->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
f1->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t1.dot(error) * invDtF32));
f1->linDeltaVA = V3LoadA(deltaV0.linear);
f1->angDeltaVA = V3LoadA(deltaV0.angular);
f1->linDeltaVB = V3LoadA(deltaV1.linear);
f1->angDeltaVB = V3LoadA(deltaV1.angular);
f1->targetVel = targetVel;
}
}
}
frictionPatchWritebackAddrIndex++;
}
}
void RenderPhysX3Debug::addConeExt(float r0, float r1, const PxVec3& p0, const PxVec3& p1 , const RendererColor& color, PxU32 renderFlags)
{
PxVec3 axis = p1 - p0;
PxReal length = axis.magnitude();
PxReal rdiff = r0 - r1;
PxReal sinAngle = rdiff / length;
PxReal x0 = r0 * sinAngle;
PxReal x1 = r1 * sinAngle;
PxVec3 center = 0.5f * (p0 + p1);
if (length < fabs(rdiff))
return;
PxReal r0p = sqrt(r0 * r0 - x0 * x0);
PxReal r1p = sqrt(r1 * r1 - x1 * x1);
if (length == 0.0f)
axis = PxVec3(1,0,0);
else
axis.normalize();
PxVec3 axis1(0.0f);
axis1[minArgument(abs(axis))] = 1.0f;
axis1 = axis1.cross(axis);
axis1.normalize();
PxVec3 axis2 = axis.cross(axis1);
axis2.normalize();
PxMat44 m;
m.column0 = PxVec4(axis, 0.0f);
m.column1 = PxVec4(axis1, 0.0f);
m.column2 = PxVec4(axis2, 0.0f);
m.column3 = PxVec4(center, 1.0f);
PxTransform tr(m);
#define NUM_CONE_VERTS 72
const PxU32 nbVerts = NUM_CONE_VERTS;
PxVec3 pts0[NUM_CONE_VERTS] ;
PxVec3 pts1[NUM_CONE_VERTS];
PxVec3 normals[NUM_CONE_VERTS] ;
const float step = PxTwoPi / float(nbVerts);
for (PxU32 i = 0; i < nbVerts; i++)
{
const float angle = float(i) * step;
const float x = cosf(angle);
const float y = sinf(angle);
PxVec3 p = PxVec3(0.0f, x, y);
pts0[i] = tr.transform(r0p * p + PxVec3(-0.5f * length + x0,0,0));
pts1[i] = tr.transform(r1p * p + PxVec3(0.5f * length + x1, 0, 0));
normals[i] = tr.q.rotate(p.getNormalized());
normals[i] = x0 * axis + r0p * normals[i];
normals[i].normalize();
}
#undef NUM_CONE_VERTS
if(renderFlags & RENDER_DEBUG_WIREFRAME)
{
for(PxU32 i=0;i<nbVerts;i++)
{
addLine(pts1[i], pts0[i], color);
}
}
if(renderFlags & RENDER_DEBUG_SOLID)
{
for(PxU32 i=0;i<nbVerts;i++)
{
const PxU32 j = (i+1) % nbVerts;
addTriangle(pts0[i], pts1[j], pts0[j], normals[i], normals[j], normals[j], color);
addTriangle(pts0[i], pts1[i], pts1[j], normals[i], normals[i], normals[j], color);
}
}
}
// PT: ray-capsule intersection code, originally from the old Magic Software library.
PxU32 Gu::intersectRayCapsuleInternal(const PxVec3& origin, const PxVec3& dir, const PxVec3& p0, const PxVec3& p1, float radius, PxReal s[2])
{
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
PxVec3 kW = p1 - p0;
const float fWLength = kW.magnitude();
if(fWLength!=0.0f)
kW /= fWLength;
// PT: if the capsule is in fact a sphere, switch back to dedicated sphere code.
// This is not just an optimization, the rest of the code fails otherwise.
if(fWLength<=1e-6f)
{
const float d0 = (origin - p0).magnitudeSquared();
const float d1 = (origin - p1).magnitudeSquared();
const float approxLength = (PxMax(d0, d1) + radius)*2.0f;
return PxU32(Gu::intersectRaySphere(origin, dir, approxLength, p0, radius, s[0]));
}
// generate orthonormal basis
PxVec3 kU(0.0f);
if (fWLength > 0.0f)
{
PxReal fInvLength;
if ( PxAbs(kW.x) >= PxAbs(kW.y) )
{
// W.x or W.z is the largest magnitude component, swap them
fInvLength = PxRecipSqrt(kW.x*kW.x + kW.z*kW.z);
kU.x = -kW.z*fInvLength;
kU.y = 0.0f;
kU.z = kW.x*fInvLength;
}
else
{
// W.y or W.z is the largest magnitude component, swap them
fInvLength = PxRecipSqrt(kW.y*kW.y + kW.z*kW.z);
kU.x = 0.0f;
kU.y = kW.z*fInvLength;
kU.z = -kW.y*fInvLength;
}
}
PxVec3 kV = kW.cross(kU);
kV.normalize(); // PT: fixed november, 24, 2004. This is a bug in Magic.
// compute intersection
PxVec3 kD(kU.dot(dir), kV.dot(dir), kW.dot(dir));
const float fDLength = kD.magnitude();
const float fInvDLength = fDLength!=0.0f ? 1.0f/fDLength : 0.0f;
kD *= fInvDLength;
const PxVec3 kDiff = origin - p0;
const PxVec3 kP(kU.dot(kDiff), kV.dot(kDiff), kW.dot(kDiff));
const PxReal fRadiusSqr = radius*radius;
// Is the velocity parallel to the capsule direction? (or zero)
if ( PxAbs(kD.z) >= 1.0f - PX_EPS_REAL || fDLength < PX_EPS_REAL )
{
const float fAxisDir = dir.dot(kW);
const PxReal fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
if ( fAxisDir < 0 && fDiscr >= 0.0f )
{
// Velocity anti-parallel to the capsule direction
const PxReal fRoot = PxSqrt(fDiscr);
s[0] = (kP.z + fRoot)*fInvDLength;
s[1] = -(fWLength - kP.z + fRoot)*fInvDLength;
return 2;
}
else if ( fAxisDir > 0 && fDiscr >= 0.0f )
{
// Velocity parallel to the capsule direction
const PxReal fRoot = PxSqrt(fDiscr);
s[0] = -(kP.z + fRoot)*fInvDLength;
s[1] = (fWLength - kP.z + fRoot)*fInvDLength;
return 2;
}
else
{
// sphere heading wrong direction, or no velocity at all
return 0;
}
}
// test intersection with infinite cylinder
PxReal fA = kD.x*kD.x + kD.y*kD.y;
PxReal fB = kP.x*kD.x + kP.y*kD.y;
PxReal fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
PxReal fDiscr = fB*fB - fA*fC;
if ( fDiscr < 0.0f )
{
// line does not intersect infinite cylinder
return 0;
}
PxU32 iQuantity = 0;
if ( fDiscr > 0.0f )
{
// line intersects infinite cylinder in two places
const PxReal fRoot = PxSqrt(fDiscr);
const PxReal fInv = 1.0f/fA;
PxReal fT = (-fB - fRoot)*fInv;
PxReal fTmp = kP.z + fT*kD.z;
const float epsilon = 1e-3f; // PT: see TA35174
if ( fTmp >= -epsilon && fTmp <= fWLength+epsilon )
s[iQuantity++] = fT*fInvDLength;
fT = (-fB + fRoot)*fInv;
fTmp = kP.z + fT*kD.z;
if ( fTmp >= -epsilon && fTmp <= fWLength+epsilon )
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
{
// line intersects capsule wall in two places
return 2;
}
}
else
{
// line is tangent to infinite cylinder
const PxReal fT = -fB/fA;
const PxReal fTmp = kP.z + fT*kD.z;
if ( 0.0f <= fTmp && fTmp <= fWLength )
{
s[0] = fT*fInvDLength;
return 1;
}
}
// test intersection with bottom hemisphere
// fA = 1
fB += kP.z*kD.z;
fC += kP.z*kP.z;
fDiscr = fB*fB - fC;
if ( fDiscr > 0.0f )
{
const PxReal fRoot = PxSqrt(fDiscr);
PxReal fT = -fB - fRoot;
PxReal fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
fT = -fB + fRoot;
fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
else if ( fDiscr == 0.0f )
{
const PxReal fT = -fB;
const PxReal fTmp = kP.z + fT*kD.z;
if ( fTmp <= 0.0f )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
// test intersection with top hemisphere
// fA = 1
fB -= kD.z*fWLength;
fC += fWLength*(fWLength - 2.0f*kP.z);
fDiscr = fB*fB - fC;
if ( fDiscr > 0.0f )
{
const PxReal fRoot = PxSqrt(fDiscr);
PxReal fT = -fB - fRoot;
PxReal fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
fT = -fB + fRoot;
fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
else if ( fDiscr == 0.0f )
{
const PxReal fT = -fB;
const PxReal fTmp = kP.z + fT*kD.z;
if ( fTmp >= fWLength )
{
s[iQuantity++] = fT*fInvDLength;
if ( iQuantity == 2 )
return 2;
}
}
return iQuantity;
}
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;
}
{
memset(normals, 0, sizeof(PxVec3)*numVerts);
const PxU32 numIndices = numFaces*3;
// accumulate area weighted face normals in each vertex
for (PxU32 t=0; t < numIndices; t+=3)
{
PxU16 i = faces[t];
PxU16 j = faces[t+1];
PxU16 k = faces[t+2];
PxVec3 e1 = vertices[j]-vertices[i];
PxVec3 e2 = vertices[k]-vertices[i];
PxVec3 n = e2.cross(e1);
normals[i] += n;
normals[j] += n;
normals[k] += n;
}
// average
for (PxU32 i=0; i < numVerts; ++i)
normals[i].normalize();
}
}
void RenderClothActor::updateRenderShape()
{
bool setupFinalizeExtSolverConstraintsCoulomb(PxcNpWorkUnit& n,
const ContactBuffer& buffer,
const PxcCorrelationBufferCoulomb& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
bool /*perPointFriction*/,
PxU8* workspace,
PxReal invDt,
PxReal bounceThreshold,
PxsSolverExtBody& b0,
PxsSolverExtBody& b1,
PxU32 frictionCountPerPoint,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
PxU8* PX_RESTRICT ptr = workspace;
const FloatV zero=FZero();
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b0.mLinkIndex];
const FloatV maxPen = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);
const FloatV restDistance = FLoad(n.restDistance);
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
bool useExtContacts = (n.flags & (PxcNpWorkUnitFlag::eARTICULATION_BODY0|PxcNpWorkUnitFlag::eARTICULATION_BODY1))!=0;
const PxU32 frictionPatchCount = c.frictionPatchCount;
const bool staticBody = ((n.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) == 0);
const PxU32 pointStride = useExtContacts ? sizeof(PxcSolverContactExt) : sizeof(PxcSolverContact);
const PxU32 frictionStride = useExtContacts ? sizeof(PxcSolverFrictionExt) : sizeof(PxcSolverFriction);
const PxU8 pointHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_CONTACT : (staticBody ? PXS_SC_TYPE_STATIC_CONTACT : PXS_SC_TYPE_RB_CONTACT));
const PxU8 frictionHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_FRICTION : (staticBody ? PXS_SC_TYPE_STATIC_FRICTION : PXS_SC_TYPE_FRICTION));
PxReal d0 = n.dominance0 * invMassScale0;
PxReal d1 = n.dominance1 * invMassScale1;
PxReal angD0 = n.dominance0 * invInertiaScale0;
PxReal angD1 = n.dominance1 * invInertiaScale1;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const PxcFrictionPatchCoulomb& frictionPatch = c.frictionPatches[i];
const Vec3V normalV = Ps::aos::V3LoadU(frictionPatch.normal);
const PxVec3 normal = frictionPatch.normal;
const PxReal combinedRestitution = contactBase0->restitution;
PxcSolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr);
ptr += sizeof(PxcSolverContactCoulombHeader);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
header->numNormalConstr = (PxU8)contactCount;
header->type = pointHeaderType;
header->setRestitution(combinedRestitution);
header->setDominance0(d0);
header->setDominance1(d1);
header->angDom0 = angD0;
header->angDom1 = angD1;
header->setNormal(normalV);
for(PxU32 patch=c.correlationListHeads[i];
patch!=PxcCorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
PxcSolverContactExt* PX_RESTRICT solverContact = reinterpret_cast<PxcSolverContactExt*>(p);
p += pointStride;
const FloatV separation = FLoad(contact.separation);
PxVec3 ra = contact.point - bodyFrame0.p;
PxVec3 rb = contact.point - bodyFrame1.p;
Vec3V targetVel = V3LoadU(contact.targetVel);
const FloatV maxImpulse = FLoad(contact.maxImpulse);
solverContact->scaledBiasX_targetVelocityY_maxImpulseZ = V3Merge(FMax(maxPen, FSub(separation, restDistance)), V3Dot(normalV,targetVel), maxImpulse);
//TODO - should we do cross only in vector land and then store. Could cause a LHS but probably no worse than
//what we already have (probably has a LHS from converting from vector to scalar above)
const PxVec3 raXn = ra.cross(normal);
const PxVec3 rbXn = rb.cross(normal);
Cm::SpatialVector deltaV0, deltaV1;
PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(normal, raXn), deltaV0, d0, angD0,
b1, Cm::SpatialVector(-normal, -rbXn), deltaV1, d1, angD1);
const PxReal vrel = b0.projectVelocity(normal, raXn)
- b1.projectVelocity(normal, rbXn);
solverContact->raXnXYZ_appliedForceW = V4SetW(Vec4V_From_Vec3V(V3LoadU(raXn)), zero);
solverContact->rbXnXYZ_velMultiplierW = V4SetW(Vec4V_From_Vec3V(V3LoadU(rbXn)), zero);
completeContactPoint(*solverContact, unitResponse, vrel, invDt, header->restitution, bounceThreshold);
solverContact->setDeltaVA(deltaV0.linear, deltaV0.angular);
solverContact->setDeltaVB(deltaV1.linear, deltaV1.angular);
}
ptr = p;
}
}
//construct all the frictions
PxU8* PX_RESTRICT ptr2 = workspace;
const PxF32 orthoThreshold = 0.70710678f;
const PxF32 eps = 0.00001f;
bool hasFriction = false;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
PxcSolverContactCoulombHeader* header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr2);
header->frictionOffset = PxU16(ptr - ptr2);
ptr2 += sizeof(PxcSolverContactCoulombHeader) + header->numNormalConstr * pointStride;
PxVec3 normal = c.frictionPatches[i].normal;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const PxReal staticFriction = contactBase0->staticFriction;
const PxU32 disableStrongFriction = contactBase0->internalFaceIndex1 & PxMaterialFlag::eDISABLE_FRICTION;
const bool haveFriction = (disableStrongFriction == 0);
PxcSolverFrictionHeader* frictionHeader = (PxcSolverFrictionHeader*)ptr;
frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatches[i].numConstraints : 0);
ptr += sizeof(PxcSolverFrictionHeader);
ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
const PxReal sqDist = t0_.dot(t0_);
const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
const PxVec3 tDir1 = tDir0.cross(normal);
PxVec3 tFallback[2] = {tDir0, tDir1};
PxU32 ind = 0;
if(haveFriction)
{
hasFriction = true;
frictionHeader->setStaticFriction(staticFriction);
frictionHeader->setDominance0(n.dominance0);
frictionHeader->setDominance1(n.dominance1);
frictionHeader->angDom0 = angD0;
frictionHeader->angDom1 = angD1;
frictionHeader->type = frictionHeaderType;
PxU32 totalPatchContactCount = 0;
for(PxU32 patch=c.correlationListHeads[i];
patch!=PxcCorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const PxU32 start = c.contactPatches[patch].start;
const Gu::ContactPoint* contactBase = buffer.contacts + start;
PxU8* p = ptr;
for(PxU32 j =0; j < count; j++)
{
const PxU32 contactId = totalPatchContactCount + j;
const Gu::ContactPoint& contact = contactBase[j];
const PxVec3 ra = contact.point - bodyFrame0.p;
const PxVec3 rb = contact.point - bodyFrame1.p;
for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
{
PxcSolverFrictionExt* PX_RESTRICT f0 = reinterpret_cast<PxcSolverFrictionExt*>(p);
p += frictionStride;
f0->contactIndex = contactId;
PxVec3 t0 = tFallback[ind];
ind = 1 - ind;
PxVec3 raXn = ra.cross(t0);
PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(t0, raXn), deltaV0, d0, angD0,
b1, Cm::SpatialVector(-t0, -rbXn), deltaV1, d1, angD1);
f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
f0->setRaXn(raXn);
f0->setRbXn(rbXn);
f0->setNormal(t0);
f0->setAppliedForce(0.0f);
f0->setDeltaVA(deltaV0.linear, deltaV0.angular);
f0->setDeltaVB(deltaV1.linear, deltaV1.angular);
}
}
totalPatchContactCount += c.contactPatches[patch].count;
ptr = p;
}
}
}
//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
return hasFriction;
}
void SampleCustomGravityCameraController::update(Camera& camera, PxReal dtime)
{
const PxExtendedVec3& currentPos = mCCT.getPosition();
const PxVec3 curPos = toVec3(currentPos);
// Compute up vector for current CCT position
PxVec3 upVector;
mBase.mPlanet.getUpVector(upVector, curPos);
PX_ASSERT(upVector.isFinite());
// Update CCT
if(!mBase.isPaused())
{
if(1)
{
bool recordPos = true;
const PxU32 currentSize = mNbRecords;
if(currentSize)
{
const PxVec3 lastPos = mHistory[currentSize-1];
// const float limit = 0.1f;
const float limit = 0.5f;
if((curPos - lastPos).magnitude()<limit)
recordPos = false;
}
if(recordPos)
{
if(mNbRecords==POS_HISTORY_LIMIT)
{
for(PxU32 i=1;i<mNbRecords;i++)
mHistory[i-1] = mHistory[i];
mNbRecords--;
}
mHistory[mNbRecords++] = curPos;
}
}
// Subtract off the 'up' component of the view direction to get our forward motion vector.
PxVec3 viewDir = camera.getViewDir();
PxVec3 forward = (viewDir - upVector * upVector.dot(viewDir)).getNormalized();
// PxVec3 forward = mForward;
// Compute "right" vector
PxVec3 right = forward.cross(upVector);
right.normalize();
// PxVec3 right = mRightV;
PxVec3 targetKeyDisplacement(0);
if(mFwd) targetKeyDisplacement += forward;
if(mBwd) targetKeyDisplacement -= forward;
if(mRight) targetKeyDisplacement += right;
if(mLeft) targetKeyDisplacement -= right;
targetKeyDisplacement *= mKeyShiftDown ? mRunningSpeed : mWalkingSpeed;
// targetKeyDisplacement += PxVec3(0,-9.81,0);
targetKeyDisplacement *= dtime;
PxVec3 targetPadDisplacement(0);
targetPadDisplacement += forward * mGamepadForwardInc * mRunningSpeed;
targetPadDisplacement += right * mGamepadLateralInc * mRunningSpeed;
// targetPadDisplacement += PxVec3(0,-9.81,0);
targetPadDisplacement *= dtime;
const PxF32 heightDelta = gJump.getHeight(dtime);
// printf("%f\n", heightDelta);
PxVec3 upDisp = upVector;
if(heightDelta!=0.0f)
upDisp *= heightDelta;
else
upDisp *= -9.81f * dtime;
const PxVec3 disp = targetKeyDisplacement + targetPadDisplacement + upDisp;
//upDisp.normalize();
//printf("%f | %f | %f\n", upDisp.x, upDisp.y, upDisp.z);
// printf("%f | %f | %f\n", targetKeyDisplacement.x, targetKeyDisplacement.y, targetKeyDisplacement.z);
// printf("%f | %f | %f\n\n", targetPadDisplacement.x, targetPadDisplacement.y, targetPadDisplacement.z);
mCCT.setUpDirection(upVector);
const PxU32 flags = mCCT.move(disp, 0.001f, dtime, PxControllerFilters());
if(flags & PxControllerFlag::eCOLLISION_DOWN)
{
gJump.stopJump();
// printf("Stop jump\n");
}
}
// Update camera
if(1)
{
mTargetYaw += mGamepadYawInc * dtime;
mTargetPitch += mGamepadPitchInc * dtime;
// Clamp pitch
// if(mTargetPitch<mPitchMin) mTargetPitch = mPitchMin;
// if(mTargetPitch>mPitchMax) mTargetPitch = mPitchMax;
}
if(1)
{
PxVec3 up = upVector;
PxQuat localPitchQ(mTargetPitch, PxVec3(1.0f, 0.0f, 0.0f));
PX_ASSERT(localPitchQ.isSane());
PxMat33 localPitchM(localPitchQ);
const PxVec3 upRef(0.0f, 1.0f, 0.0f);
PxQuat localYawQ(mTargetYaw, upRef);
PX_ASSERT(localYawQ.isSane());
PxMat33 localYawM(localYawQ);
bool res;
PxQuat localToWorldQ = rotationArc(upRef, up, res);
static PxQuat memory(0,0,0,1);
if(!res)
{
localToWorldQ = memory;
}
else
{
memory = localToWorldQ;
}
PX_ASSERT(localToWorldQ.isSane());
PxMat33 localToWorld(localToWorldQ);
static PxVec3 previousUp(0.0f, 1.0f, 0.0f);
static PxQuat incLocalToWorldQ(0.0f, 0.0f, 0.0f, 1.0f);
PxQuat incQ = rotationArc(previousUp, up, res);
PX_ASSERT(incQ.isSane());
// incLocalToWorldQ = incLocalToWorldQ * incQ;
incLocalToWorldQ = incQ * incLocalToWorldQ;
PX_ASSERT(incLocalToWorldQ.isSane());
incLocalToWorldQ.normalize();
PxMat33 incLocalToWorldM(incLocalToWorldQ);
localToWorld = incLocalToWorldM;
previousUp = up;
mTest = localToWorld;
//mTest = localToWorld * localYawM;
// PxMat33 rot = localYawM * localToWorld;
PxMat33 rot = localToWorld * localYawM * localPitchM;
// PxMat33 rot = localToWorld * localYawM;
PX_ASSERT(rot.column0.isFinite());
PX_ASSERT(rot.column1.isFinite());
PX_ASSERT(rot.column2.isFinite());
////
PxMat44 view(rot.column0, rot.column1, rot.column2, PxVec3(0));
mForward = -rot.column2;
mRightV = rot.column0;
camera.setView(PxTransform(view));
PxVec3 viewDir = camera.getViewDir();
PX_ASSERT(viewDir.isFinite());
////
PxRigidActor* characterActor = mCCT.getActor();
PxShape* shape;
characterActor->getShapes(&shape,1);
PxCapsuleGeometry geom;
shape->getCapsuleGeometry(geom);
up *= geom.halfHeight+geom.radius;
const PxVec3 headPos = curPos + up;
const float distanceToTarget = 10.0f;
// const float distanceToTarget = 20.0f;
// const float distanceToTarget = 5.0f;
// const PxVec3 camPos = headPos - viewDir*distanceToTarget;
const PxVec3 camPos = headPos - mForward*distanceToTarget;// + up * 20.0f;
// view.t = camPos;
view.column3 = PxVec4(camPos,0);
// camera.setView(view);
camera.setView(PxTransform(view));
mTarget = headPos;
}
if(0)
{
PxControllerState cctState;
mCCT.getState(cctState);
printf("\nCCT state:\n");
printf("delta: %.02f | %.02f | %.02f\n", cctState.deltaXP.x, cctState.deltaXP.y, cctState.deltaXP.z);
printf("touchedShape: %p\n", cctState.touchedShape);
printf("touchedObstacle: %p\n", cctState.touchedObstacle);
printf("standOnAnotherCCT: %d\n", cctState.standOnAnotherCCT);
printf("standOnObstacle: %d\n", cctState.standOnObstacle);
printf("isMovingUp: %d\n", cctState.isMovingUp);
printf("collisionFlags: %d\n", cctState.collisionFlags);
}
}
