void FPhysScene::SetKinematicTarget(FBodyInstance * BodyInstance, const FTransform & TargetTransform)
{
TargetTransform.DiagnosticCheckNaN_All();
#if WITH_PHYSX
if (PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic())
{
#if WITH_SUBSTEPPING
if (IsSubstepping())
{
FPhysSubstepTask * PhysSubStepper = PhysSubSteppers[SceneType(BodyInstance)];
PhysSubStepper->SetKinematicTarget(BodyInstance, TargetTransform);
}
else
#endif
{
const PxTransform PNewPose = U2PTransform(TargetTransform);
check(PNewPose.isValid());
SCOPED_SCENE_WRITE_LOCK(PRigidDynamic->getScene());
PRigidDynamic->setKinematicTarget(PNewPose);
}
}
#endif
}
void NpRigidDynamic::setGlobalPose(const PxTransform& pose, bool autowake)
{
NpScene* scene = NpActor::getAPIScene(*this);
#ifdef PX_CHECKED
if(scene)
scene->checkPositionSanity(*this, pose, "PxRigidDynamic::setGlobalPose");
#endif
PX_CHECK_AND_RETURN(pose.isSane(), "NpRigidDynamic::setGlobalPose: pose is not valid.");
NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
PxTransform p = pose.getNormalized();
if(scene)
{
updateDynamicSceneQueryShapes(mShapeManager, scene->getSceneQueryManagerFast());
}
PxTransform newPose = p;
newPose.q.normalize(); //AM: added to fix 1461 where users read and write orientations for no reason.
Scb::Body& b = getScbBodyFast();
PxTransform body2World = newPose * b.getBody2Actor();
b.setBody2World(body2World, false);
if(scene && autowake && !(b.getActorFlags() & PxActorFlag::eDISABLE_SIMULATION))
wakeUpInternal();
}
static PxTransform computeBoneTransform(PxTransform &rootTransform, Acclaim::Bone &bone, PxVec3* boneFrameData)
{
using namespace Acclaim;
//PxTransform rootTransform(PxVec3(0.0f), PxQuat(PxIdentity));
PxTransform parentTransform = (bone.mParent) ?
computeBoneTransform(rootTransform, *bone.mParent, boneFrameData) : rootTransform;
PxQuat qWorld = EulerAngleToQuat(bone.mAxis);
PxVec3 offset = bone.mLength * bone.mDirection;
PxQuat qDelta = PxQuat(PxIdentity);
PxVec3 boneData = boneFrameData[bone.mID-1];
if (bone.mDOF & BoneDOFFlag::eRX)
qDelta = PxQuat(Ps::degToRad(boneData.x), PxVec3(1.0f, 0.0f, 0.0f)) * qDelta;
if (bone.mDOF & BoneDOFFlag::eRY)
qDelta = PxQuat(Ps::degToRad(boneData.y), PxVec3(0.0f, 1.0f, 0.0f)) * qDelta;
if (bone.mDOF & BoneDOFFlag::eRZ)
qDelta = PxQuat(Ps::degToRad(boneData.z), PxVec3(0.0f, 0.0f, 1.0f)) * qDelta;
PxQuat boneOrientation = qWorld * qDelta * qWorld.getConjugate();
PxTransform boneTransform(boneOrientation.rotate(offset), boneOrientation);
return parentTransform.transform(boneTransform);
}
/** Interpolates kinematic actor transform - Assumes caller has obtained writer lock */
void FPhysSubstepTask::InterpolateKinematicActor_AssumesLocked(const FPhysTarget& PhysTarget, FBodyInstance* BodyInstance, float InAlpha)
{
#if WITH_PHYSX
PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic_AssumesLocked();
InAlpha = FMath::Clamp(InAlpha, 0.f, 1.f);
/** Interpolate kinematic actors */
if (PhysTarget.bKinematicTarget)
{
//It's possible that the actor is no longer kinematic and is now simulating. In that case do nothing
if (!BodyInstance->IsNonKinematic())
{
const FKinematicTarget& KinematicTarget = PhysTarget.KinematicTarget;
const FTransform& TargetTM = KinematicTarget.TargetTM;
const FTransform& StartTM = KinematicTarget.OriginalTM;
FTransform InterTM = FTransform::Identity;
InterTM.SetLocation(FMath::Lerp(StartTM.GetLocation(), TargetTM.GetLocation(), InAlpha));
InterTM.SetRotation(FMath::Lerp(StartTM.GetRotation(), TargetTM.GetRotation(), InAlpha));
const PxTransform PNewPose = U2PTransform(InterTM);
check(PNewPose.isValid());
PRigidDynamic->setKinematicTarget(PNewPose);
}
}
#endif
}
/** Interpolates kinematic actor transform - Assumes caller has obtained writer lock */
void FPhysSubstepTask::InterpolateKinematicActor(const FPhysTarget & PhysTarget, FBodyInstance * BodyInstance, float Alpha)
{
#if WITH_PHYSX
PxRigidDynamic * PRigidDynamic = BodyInstance->GetPxRigidDynamic();
Alpha = FMath::Clamp(Alpha, 0.f, 1.f);
/** Interpolate kinematic actors */
if (PhysTarget.bKinematicTarget)
{
//We should only be interpolating kinematic actors
check(!IsRigidDynamicNonKinematic(PRigidDynamic));
const FKinematicTarget & KinematicTarget = PhysTarget.KinematicTarget;
const FTransform & TargetTM = KinematicTarget.TargetTM;
const FTransform & StartTM = KinematicTarget.OriginalTM;
FTransform InterTM = FTransform::Identity;
InterTM.SetLocation(FMath::Lerp(StartTM.GetLocation(), TargetTM.GetLocation(), Alpha));
InterTM.SetRotation(FMath::Lerp(StartTM.GetRotation(), TargetTM.GetRotation(), Alpha));
const PxTransform PNewPose = U2PTransform(InterTM);
check(PNewPose.isValid());
PRigidDynamic->setKinematicTarget(PNewPose);
}
#endif
}
void SampleVehicle::drawWheels()
{
const RendererColor colorPurple(255, 0, 255);
for(PxU32 i=0;i<mVehicleManager.getNbVehicles();i++)
{
//Draw a rotating arrow to get an idea of the wheel rotation speed.
PxVehicleWheels* veh=mVehicleManager.getVehicle(i);
const PxRigidDynamic* actor=veh->getRigidDynamicActor();
PxShape* shapeBuffer[PX_MAX_NUM_WHEELS];
actor->getShapes(shapeBuffer,veh->mWheelsSimData.getNumWheels());
const PxTransform vehGlobalPose=actor->getGlobalPose();
const PxU32 numWheels=veh->mWheelsSimData.getNumWheels();
for(PxU32 j=0;j<numWheels;j++)
{
const PxTransform wheelTransform=vehGlobalPose.transform(shapeBuffer[j]->getLocalPose());
const PxF32 wheelRadius=veh->mWheelsSimData.getWheelData(j).mRadius;
const PxF32 wheelHalfWidth=veh->mWheelsSimData.getWheelData(j).mWidth*0.5f;
PxVec3 offset=wheelTransform.q.getBasisVector0()*wheelHalfWidth;
offset*= (veh->mWheelsSimData.getWheelCentreOffset(j).x > 0) ? 1.0f : -1.0f;
const PxVec3 arrow=wheelTransform.rotate(PxVec3(0,0,1));
getDebugRenderer()->addLine(wheelTransform.p+offset, wheelTransform.p+offset+arrow*wheelRadius, colorPurple);
}
}
}
bool physx::Gu::computePlane_ConvexMTD(const PxPlane& plane, const PxConvexMeshGeometry& convexGeom, const PxTransform& convexPose, PxSweepHit& hit)
{
const ConvexMesh* convexMesh = static_cast<const ConvexMesh*>(convexGeom.convexMesh);
const Cm::FastVertex2ShapeScaling convexScaling(convexGeom.scale);
PxU32 nbVerts = convexMesh->getNbVerts();
const PxVec3* PX_RESTRICT verts = convexMesh->getVerts();
PxVec3 worldPointMin = convexPose.transform(convexScaling * verts[0]);
PxReal dmin = plane.distance(worldPointMin);
for(PxU32 i=1;i<nbVerts;i++)
{
const PxVec3 worldPoint = convexPose.transform(convexScaling * verts[i]);
const PxReal d = plane.distance(worldPoint);
if(dmin > d)
{
dmin = d;
worldPointMin = worldPoint;
}
}
hit.normal = plane.n;
hit.distance = dmin;
hit.position = worldPointMin - plane.n * dmin;
return true;
}
void PxRigidBodyExt::addLocalForceAtLocalPos(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
{
const PxTransform globalPose = body.getGlobalPose();
const PxVec3 globalForcePos = globalPose.transform(pos);
const PxVec3 globalForce = globalPose.rotate(force);
addForceAtPosInternal(body, globalForce, globalForcePos, mode, wakeup);
}
PxVec3 PxRigidBodyExt::getLocalVelocityAtLocalPos(const PxRigidBody& body, const PxVec3& point)
{
const PxTransform globalPose = body.getGlobalPose();
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
const PxVec3 rpoint = globalPose.transform(point) - centerOfMass;
return getVelocityAtPosInternal(body, rpoint);
}
PxVec3 PxRigidBodyExt::getVelocityAtOffset(const PxRigidBody& body, const PxVec3& point)
{
const PxTransform globalPose = body.getGlobalPose();
const PxVec3 centerOfMass = globalPose.rotate(body.getCMassLocalPose().p);
const PxVec3 rpoint = point - centerOfMass;
return getVelocityAtPosInternal(body, rpoint);
}
void NpArticulationJoint::setChildPose(const PxTransform& t)
{
PX_CHECK_AND_RETURN(t.isSane(), "NpArticulationJoint::setChildPose t is not valid.");
NP_WRITE_CHECK(getOwnerScene());
mJoint.setChildPose(mChild->getCMassLocalPose().transformInv(t.getNormalized()));
}
void NpCloth::setTargetPose(const PxTransform& pose)
{
NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
PX_CHECK_AND_RETURN(pose.isSane(), "PxCloth::setTargetPose: invalid transform!");
mCloth.setTargetPose(pose.getNormalized());
sendPvdSimpleProperties();
}
void AddConvexElemsToRigidActor_AssumesLocked() const
{
float ContactOffsetFactor, MaxContactOffset;
GetContactOffsetParams(ContactOffsetFactor, MaxContactOffset);
for (int32 i = 0; i < BodySetup->AggGeom.ConvexElems.Num(); i++)
{
const FKConvexElem& ConvexElem = BodySetup->AggGeom.ConvexElems[i];
if (ConvexElem.ConvexMesh)
{
PxTransform PLocalPose;
bool bUseNegX = CalcMeshNegScaleCompensation(Scale3D, PLocalPose);
PxConvexMeshGeometry PConvexGeom;
PConvexGeom.convexMesh = bUseNegX ? ConvexElem.ConvexMeshNegX : ConvexElem.ConvexMesh;
PConvexGeom.scale.scale = U2PVector(Scale3DAbs * ConvexElem.GetTransform().GetScale3D().GetAbs());
FTransform ConvexTransform = ConvexElem.GetTransform();
if (ConvexTransform.GetScale3D().X < 0 || ConvexTransform.GetScale3D().Y < 0 || ConvexTransform.GetScale3D().Z < 0)
{
UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] has negative scale. Not currently supported"), *GetPathNameSafe(BodySetup->GetOuter()), i);
}
if (ConvexTransform.IsValid())
{
PxTransform PElementTransform = U2PTransform(ConvexTransform * RelativeTM);
PLocalPose.q *= PElementTransform.q;
PLocalPose.p = PElementTransform.p;
PLocalPose.p.x *= Scale3D.X;
PLocalPose.p.y *= Scale3D.Y;
PLocalPose.p.z *= Scale3D.Z;
if (PConvexGeom.isValid())
{
PxVec3 PBoundsExtents = PConvexGeom.convexMesh->getLocalBounds().getExtents();
ensure(PLocalPose.isValid());
{
const float ContactOffset = FMath::Min(MaxContactOffset, ContactOffsetFactor * PBoundsExtents.minElement());
AttachShape_AssumesLocked(PConvexGeom, PLocalPose, ContactOffset);
}
}
else
{
UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] invalid"), *GetPathNameSafe(BodySetup->GetOuter()), i);
}
}
else
{
UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: [%s] ConvexElem[%d] has invalid transform"), *GetPathNameSafe(BodySetup->GetOuter()), i);
}
}
else
{
UE_LOG(LogPhysics, Warning, TEXT("AddConvexElemsToRigidActor: ConvexElem is missing ConvexMesh (%d: %s)"), i, *BodySetup->GetPathName());
}
}
}
void RenderClothActor::update(float deltaTime)
{
updateRenderShape();
// update collision shapes
PxU32 numCollisionSpheres = mCloth.getNbCollisionSpheres();
PxU32 numPairs = mCloth.getNbCollisionSpherePairs();
PxU32 numPlanes = mCloth.getNbCollisionPlanes();
PxU32 numConvexes = mCloth.getNbCollisionConvexes();
if (numCollisionSpheres == 0 && numConvexes == 0)
return;
if(numPlanes != mNumPlanes)
mPlanes = (PxClothCollisionPlane*)SAMPLE_ALLOC(sizeof(PxClothCollisionPlane) * (mNumPlanes = numPlanes));
if(numConvexes != mNumConvexes)
mConvexMasks = (PxU32*)SAMPLE_ALLOC(sizeof(PxU32) * (mNumConvexes = numConvexes));
mCloth.getCollisionData(mSpheres, mIndexPairs, mPlanes, mConvexMasks);
PxTransform clothPose = mCloth.getGlobalPose();
// update all spheres
for (PxU32 i=0; i < mSphereActors.size(); ++i)
{
float r = mSpheres[i].radius*mCapsuleScale;
mSphereActors[i]->setRendering(true);
mSphereActors[i]->setTransform(PxTransform(clothPose.transform(mSpheres[i].pos)));
mSphereActors[i]->setMeshScale(PxMeshScale(PxVec3(r, r, r), PxQuat::createIdentity()));
}
// capsule needs to be flipped to match PxTransformFromSegment
PxTransform flip(PxVec3(0.0f), PxQuat(-PxHalfPi, PxVec3(0.0f, 0.0f, 1.0f)));
// update all capsules
for (PxU32 i=0; i < mCapsuleActors.size(); ++i)
{
PxU32 i0 = mIndexPairs[i*2];
PxU32 i1 = mIndexPairs[i*2+1];
PxClothCollisionSphere& sphere0 = mSpheres[i0];
PxClothCollisionSphere& sphere1 = mSpheres[i1];
// disable individual rendering for spheres belonging to a capsule
mSphereActors[i0]->setRendering(false);
mSphereActors[i1]->setRendering(false);
PxVec3 p0 = clothPose.transform(sphere0.pos);
PxVec3 p1 = clothPose.transform(sphere1.pos);
mCapsuleActors[i]->setDimensions((p1-p0).magnitude()*0.5f, mCapsuleScale*sphere0.radius, mCapsuleScale*sphere1.radius);
mCapsuleActors[i]->setTransform(PxTransformFromSegment(p0, p1)*flip);
}
}
static FVector FindBoxOpposingNormal(const PxLocationHit& PHit, const FVector& TraceDirectionDenorm, const FVector InNormal)
{
// We require normal info for our algorithm.
const bool bNormalData = (PHit.flags & PxHitFlag::eNORMAL);
if (!bNormalData)
{
return InNormal;
}
PxBoxGeometry PxBoxGeom;
const bool bReadGeomSuccess = PHit.shape->getBoxGeometry(PxBoxGeom);
check(bReadGeomSuccess); // This function should only be used for box geometry
const PxTransform LocalToWorld = PxShapeExt::getGlobalPose(*PHit.shape, *PHit.actor);
// Find which faces were included in the contact normal, and for multiple faces, use the one most opposing the sweep direction.
const PxVec3 ContactNormalLocal = LocalToWorld.rotateInv(PHit.normal);
const float* ContactNormalLocalPtr = &ContactNormalLocal.x;
const PxVec3 TraceDirDenormWorld = U2PVector(TraceDirectionDenorm);
const float* TraceDirDenormWorldPtr = &TraceDirDenormWorld.x;
const PxVec3 TraceDirDenormLocal = LocalToWorld.rotateInv(TraceDirDenormWorld);
const float* TraceDirDenormLocalPtr = &TraceDirDenormLocal.x;
PxVec3 BestLocalNormal(ContactNormalLocal);
float* BestLocalNormalPtr = &BestLocalNormal.x;
float BestOpposingDot = FLT_MAX;
for (int32 i=0; i < 3; i++)
{
// Select axis of face to compare to, based on normal.
if (ContactNormalLocalPtr[i] > KINDA_SMALL_NUMBER)
{
const float TraceDotFaceNormal = TraceDirDenormLocalPtr[i]; // TraceDirDenormLocal.dot(BoxFaceNormal)
if (TraceDotFaceNormal < BestOpposingDot)
{
BestOpposingDot = TraceDotFaceNormal;
BestLocalNormal = PxVec3(0.f);
BestLocalNormalPtr[i] = 1.f;
}
}
else if (ContactNormalLocalPtr[i] < -KINDA_SMALL_NUMBER)
{
const float TraceDotFaceNormal = -TraceDirDenormLocalPtr[i]; // TraceDirDenormLocal.dot(BoxFaceNormal)
if (TraceDotFaceNormal < BestOpposingDot)
{
BestOpposingDot = TraceDotFaceNormal;
BestLocalNormal = PxVec3(0.f);
BestLocalNormalPtr[i] = -1.f;
}
}
}
// Fill in result
const PxVec3 WorldNormal = LocalToWorld.rotate(BestLocalNormal);
return P2UVector(WorldNormal);
}
void NpArticulationJoint::setParentPose(const PxTransform& t)
{
PX_CHECK_AND_RETURN(t.isSane(), "NpArticulationJoint::setParentPose t is not valid.");
NP_WRITE_CHECK(getOwnerScene());
if(mParent==NULL)
return;
mJoint.setParentPose(mParent->getCMassLocalPose().transformInv(t.getNormalized()));
}
PX_FORCE_INLINE void setConstraintData(PxsParticleCollData& collData,
const PxReal& distToSurface,
const PxVec3& normal,
const PxVec3& position,
const PxTransform& shape2World)
{
PxU32 i;
if (!(collData.particleFlags.low & PxvInternalParticleFlag::eCONSTRAINT_0_VALID))
{
i = 0;
}
else if (!(collData.particleFlags.low & PxvInternalParticleFlag::eCONSTRAINT_1_VALID))
{
i = 1;
}
else
{
PxVec3 oldWorldSurfacePos(shape2World.transform(collData.localOldPos));
PxReal dist0 = collData.c0->normal.dot(oldWorldSurfacePos) - collData.c0->d;
PxReal dist1 = collData.c1->normal.dot(oldWorldSurfacePos) - collData.c1->d;
if ( dist0 < dist1)
{
if (distToSurface < dist1)
i = 1;
else
return;
}
else if (distToSurface < dist0)
{
i = 0;
}
else
return;
}
PxVec3 newSurfaceNormal(shape2World.rotate(normal));
PxVec3 newSurfacePos(shape2World.transform(position));
PxsFluidConstraint cN(newSurfaceNormal, newSurfacePos);
if(i == 0)
{
*collData.c0 = cN;
collData.particleFlags.low |= PxvInternalParticleFlag::eCONSTRAINT_0_VALID;
collData.particleFlags.low &= PxU16(~PxvInternalParticleFlag::eCONSTRAINT_0_DYNAMIC);
}
else
{
*collData.c1 = cN;
collData.particleFlags.low |= PxvInternalParticleFlag::eCONSTRAINT_1_VALID;
collData.particleFlags.low &= PxU16(~PxvInternalParticleFlag::eCONSTRAINT_1_DYNAMIC);
}
}
void UBodySetup::AddTriMeshToRigidActor(PxRigidActor* PDestActor, const FVector& Scale3D, const FVector& Scale3DAbs) const
{
float ContactOffsetFactor, MaxContactOffset;
GetContactOffsetParams(ContactOffsetFactor, MaxContactOffset);
PxMaterial* PDefaultMat = GetDefaultPhysMaterial();
if(TriMesh || TriMeshNegX)
{
PxTransform PLocalPose;
bool bUseNegX = CalcMeshNegScaleCompensation(Scale3D, PLocalPose);
// Only case where TriMeshNegX should be null is BSP, which should not require negX version
if (bUseNegX && TriMeshNegX == NULL)
{
UE_LOG(LogPhysics, Log, TEXT("AddTriMeshToRigidActor: Want to use NegX but it doesn't exist! %s"), *GetPathName());
}
PxTriangleMesh* UseTriMesh = bUseNegX ? TriMeshNegX : TriMesh;
if (UseTriMesh != NULL)
{
PxTriangleMeshGeometry PTriMeshGeom;
PTriMeshGeom.triangleMesh = bUseNegX ? TriMeshNegX : TriMesh;
PTriMeshGeom.scale.scale = U2PVector(Scale3DAbs);
if (bDoubleSidedGeometry)
{
PTriMeshGeom.meshFlags |= PxMeshGeometryFlag::eDOUBLE_SIDED;
}
if (PTriMeshGeom.isValid())
{
ensure(PLocalPose.isValid());
// Create without 'sim shape' flag, problematic if it's kinematic, and it gets set later anyway.
PxShape* NewShape = PDestActor->createShape(PTriMeshGeom, *PDefaultMat, PxShapeFlag::eSCENE_QUERY_SHAPE | PxShapeFlag::eVISUALIZATION);
if (NewShape)
{
NewShape->setLocalPose(PLocalPose);
NewShape->setContactOffset(MaxContactOffset);
}
else
{
UE_LOG(LogPhysics, Log, TEXT("Can't create new mesh shape in AddShapesToRigidActor"));
}
}
else
{
UE_LOG(LogPhysics, Log, TEXT("AddTriMeshToRigidActor: TriMesh invalid"));
}
}
}
}
static PxTransform computeBoneTransformRest(Acclaim::Bone &bone)
{
using namespace Acclaim;
PxTransform parentTransform = (bone.mParent) ?
computeBoneTransformRest(*bone.mParent) : PxTransform(PxVec3(0.0f), PxQuat(PxIdentity));
PxVec3 offset = bone.mLength * bone.mDirection;
PxTransform boneTransform(offset, PxQuat(PxIdentity));
return parentTransform.transform(boneTransform);
}
Vector3 PhysXRigidbody::getVelocityAtPoint(const Vector3& point) const
{
const PxVec3& pxPoint = toPxVector(point);
const PxTransform globalPose = mInternal->getGlobalPose();
const PxVec3 centerOfMass = globalPose.transform(mInternal->getCMassLocalPose().p);
const PxVec3 rpoint = pxPoint - centerOfMass;
PxVec3 velocity = mInternal->getLinearVelocity();
velocity += mInternal->getAngularVelocity().cross(rpoint);
return fromPxVector(velocity);
}
PxU32 physx::PxFindFaceIndex(const PxConvexMeshGeometry& convexGeom, const PxTransform& pose,
const PxVec3& impactPos, const PxVec3& unitDir)
{
PX_ASSERT(unitDir.isFinite());
PX_ASSERT(unitDir.isNormalized());
PX_ASSERT(impactPos.isFinite());
PX_ASSERT(pose.isFinite());
const PxVec3 impact = impactPos - unitDir * gEpsilon;
const PxVec3 localPoint = pose.transformInv(impact);
const PxVec3 localDir = pose.rotateInv(unitDir);
// Create shape to vertex scale transformation matrix
const PxMeshScale& meshScale = convexGeom.scale;
const PxMat33 rot(meshScale.rotation);
PxMat33 shape2VertexSkew = rot.getTranspose();
const PxMat33 diagonal = PxMat33::createDiagonal(PxVec3(1.0f / meshScale.scale.x, 1.0f / meshScale.scale.y, 1.0f / meshScale.scale.z));
shape2VertexSkew = shape2VertexSkew * diagonal;
shape2VertexSkew = shape2VertexSkew * rot;
const PxU32 nbPolys = convexGeom.convexMesh->getNbPolygons();
PxU32 minIndex = 0;
PxReal minD = PX_MAX_REAL;
for (PxU32 j = 0; j < nbPolys; j++)
{
PxHullPolygon hullPolygon;
convexGeom.convexMesh->getPolygonData(j, hullPolygon);
// transform hull plane into shape space
PxPlane plane;
const PxVec3 tmp = shape2VertexSkew.transformTranspose(PxVec3(hullPolygon.mPlane[0],hullPolygon.mPlane[1],hullPolygon.mPlane[2]));
const PxReal denom = 1.0f / tmp.magnitude();
plane.n = tmp * denom;
plane.d = hullPolygon.mPlane[3] * denom;
PxReal d = plane.distance(localPoint);
if (d < 0.0f)
continue;
const PxReal tweak = plane.n.dot(localDir) * gEpsilon;
d += tweak;
if (d < minD)
{
minIndex = j;
minD = d;
}
}
return minIndex;
}
bool
Skin::computeNewPositions(Character &character, SampleArray<PxVec3> &particlePositions)
{
if (character.mCurrentMotion == NULL)
return false;
PxTransform t = character.mCurrentBoneTransform[mBoneID];
particlePositions.resize(mBindPos.size());
for (PxU32 i = 0; i < mBindPos.size(); i++)
particlePositions[i] = t.transform(mBindPos[i]);
return true;
}
void PhysXRigidbody::addForceAtPoint(const Vector3& force, const Vector3& position, PointForceMode mode)
{
const PxVec3& pxForce = toPxVector(force);
const PxVec3& pxPos = toPxVector(position);
const PxTransform globalPose = mInternal->getGlobalPose();
PxVec3 centerOfMass = globalPose.transform(mInternal->getCMassLocalPose().p);
PxForceMode::Enum pxMode = toPxForceMode(mode);
PxVec3 torque = (pxPos - centerOfMass).cross(pxForce);
mInternal->addForce(pxForce, pxMode);
mInternal->addTorque(torque, pxMode);
}
void defaultCCTInteraction(const PxControllerShapeHit& hit)
{
PxRigidDynamic* actor = hit.shape->getActor()->is<PxRigidDynamic>();
if(actor)
{
if(actor->getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC)
return;
if(0)
{
const PxVec3 p = actor->getGlobalPose().p + hit.dir * 10.0f;
PxShape* shape;
actor->getShapes(&shape, 1);
PxRaycastHit newHit;
PxU32 n = PxShapeExt::raycast(*shape, *shape->getActor(), p, -hit.dir, 20.0f, PxHitFlag::ePOSITION, 1, &newHit, false);
if(n)
{
// We only allow horizontal pushes. Vertical pushes when we stand on dynamic objects creates
// useless stress on the solver. It would be possible to enable/disable vertical pushes on
// particular objects, if the gameplay requires it.
const PxVec3 upVector = hit.controller->getUpDirection();
const PxF32 dp = hit.dir.dot(upVector);
// shdfnd::printFormatted("%f\n", fabsf(dp));
if(fabsf(dp)<1e-3f)
// if(hit.dir.y==0.0f)
{
const PxTransform globalPose = actor->getGlobalPose();
const PxVec3 localPos = globalPose.transformInv(newHit.position);
::addForceAtLocalPos(*actor, hit.dir*hit.length*1000.0f, localPos, PxForceMode::eACCELERATION);
}
}
}
// We only allow horizontal pushes. Vertical pushes when we stand on dynamic objects creates
// useless stress on the solver. It would be possible to enable/disable vertical pushes on
// particular objects, if the gameplay requires it.
const PxVec3 upVector = hit.controller->getUpDirection();
const PxF32 dp = hit.dir.dot(upVector);
// shdfnd::printFormatted("%f\n", fabsf(dp));
if(fabsf(dp)<1e-3f)
// if(hit.dir.y==0.0f)
{
const PxTransform globalPose = actor->getGlobalPose();
const PxVec3 localPos = globalPose.transformInv(toVec3(hit.worldPos));
::addForceAtLocalPos(*actor, hit.dir*hit.length*1000.0f, localPos, PxForceMode::eACCELERATION);
}
}
}
AICollisionEntity PhysicsEngine::AISweep(Vehicle* vehicle)
{
PxTransform transform = vehicle->getActor()->getGlobalPose();
PxF32 xOffset = (vehicle->tuning.chassisDims.x * 0.5f) + 0.1f;
PxF32 zOffset = (vehicle->tuning.chassisDims.z * 0.5f) + 0.1f;
PxVec3 offset1 = transform.rotate(PxVec3(-xOffset, 0, zOffset));
PxVec3 offset2 = transform.rotate(PxVec3(xOffset, 0, zOffset));
PxVec3 origin1 = offset1 + transform.p;
PxVec3 origin2 = offset2 + transform.p;
PxVec3 direction = transform.rotate(PxVec3(0, 0, 1));
PxF32 distance = 10;
PxRaycastBuffer buffer1;
PxRaycastBuffer buffer2;
scene->raycast(origin1, direction, distance, buffer1);
scene->raycast(origin2, direction, distance, buffer2);
AICollisionEntity toReturn = AICollisionEntity();
toReturn.entity = nullptr;
if (buffer1.hasBlock)
{
PxRaycastHit hit = buffer1.block;
PxVec3 actorCentre = hit.actor->getGlobalPose().p;
PxVec3 hitPoint = hit.position;
toReturn.radius = (hitPoint - actorCentre).magnitude();
toReturn.pos = glm::vec3(hitPoint.x, hitPoint.y, hitPoint.z);
toReturn.entity = (Entity*)hit.actor->userData;
toReturn.distance = hit.distance;
}
else if (buffer2.hasBlock)
{
PxRaycastHit hit = buffer2.block;
PxVec3 actorCentre = hit.actor->getGlobalPose().p;
PxVec3 hitPoint = hit.position;
toReturn.radius = (hitPoint - actorCentre).magnitude();
toReturn.pos = glm::vec3(hitPoint.x, hitPoint.y, hitPoint.z);
toReturn.entity = (Entity*)hit.actor->userData;
toReturn.distance = hit.distance;
}
return toReturn;
}
PX_INLINE void addForceAtPosInternal(PxRigidBody& body, const PxVec3& force, const PxVec3& pos, PxForceMode::Enum mode, bool wakeup)
{
if(mode == PxForceMode::eACCELERATION || mode == PxForceMode::eVELOCITY_CHANGE)
{
Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__,
"PxRigidBodyExt::addForce methods do not support eACCELERATION or eVELOCITY_CHANGE modes");
return;
}
const PxTransform globalPose = body.getGlobalPose();
const PxVec3 centerOfMass = globalPose.transform(body.getCMassLocalPose().p);
const PxVec3 torque = (pos - centerOfMass).cross(force);
body.addForce(force, mode, wakeup);
body.addTorque(torque, mode, wakeup);
}
bool Gu::intersectBoxConvex(const PxBoxGeometry& boxGeom, const PxTransform& boxGlobalPose,
const Gu::ConvexMesh& mesh, const PxMeshScale& meshScale, const PxTransform& convexGlobalPose,
PxVec3*)
{
// AP: see archived non-GJK version in //sw/physx/dev/pterdiman/graveyard/contactConvexBox.cpp
using namespace Ps::aos;
const Vec3V zeroV = V3Zero();
const Gu::ConvexHullData* hull = &mesh.getHull();
const Vec3V vScale = V3LoadU(meshScale.scale);
const QuatV vQuat = QuatVLoadU(&meshScale.rotation.x);
const Vec3V boxExtents = V3LoadU(boxGeom.halfExtents);
const PsMatTransformV aToB(convexGlobalPose.transformInv(boxGlobalPose));
const Gu::ConvexHullV convexHull(hull, zeroV, vScale, vQuat);
const Gu::BoxV box(zeroV, boxExtents);
/*const FloatV tolerance = FAdd(box.getMargin(), convexHull.getMargin());
const FloatV sqTolerance = FMul(tolerance, tolerance);*/
const FloatV convexTolerance = CalculateConvexTolerance(hull, vScale);
const FloatV boxTolerance = CalculateBoxTolerance(boxExtents);
const FloatV tolerance = FAdd(convexTolerance, boxTolerance);
const FloatV sqTolerance = FMul(tolerance, tolerance);
Vec3V contactA, contactB, normal;
FloatV sqDist;
PxGJKStatus status = Gu::GJKRelativeTesselation(box, convexHull, aToB, sqTolerance, contactA, contactB, normal, sqDist);
//const bool overlap = status == GJK_CONTACT;
//PX_PRINTF("BOX status = %i, overlap = %i, PxVec3(%f, %f, %f)\n", status, overlap, boxGlobalPose.p.x, boxGlobalPose.p.y, boxGlobalPose.p.z);
return status == GJK_CONTACT;
}
bool Gu::intersectCapsuleConvex(const PxCapsuleGeometry& capsGeom, const PxTransform& capsGlobalPose,
const Gu::ConvexMesh& mesh, const PxMeshScale& meshScale, const PxTransform& convexGlobalPose,
PxVec3*)
{
using namespace Ps::aos;
const Vec3V zeroV = V3Zero();
const Gu::ConvexHullData* hull = &mesh.getHull();
const FloatV capsuleHalfHeight = FLoad(capsGeom.halfHeight);
const FloatV capsuleRadius = FLoad(capsGeom.radius);
const Vec3V vScale = V3LoadU(meshScale.scale);
const QuatV vQuat = QuatVLoadU(&meshScale.rotation.x);
const PsMatTransformV aToB(convexGlobalPose.transformInv(capsGlobalPose));
const Gu::ConvexHullV convexHull(hull, zeroV, vScale, vQuat);
const Gu::CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
//const Gu::CapsuleV capsule(zeroV, V3Scale(V3UnitX(), capsuleHalfHeight), capsuleRadius);
Vec3V contactA, contactB, normal;
FloatV sqDist;
PxGJKStatus status = Gu::GJKLocal(capsule, convexHull, contactA, contactB, normal, sqDist);
const FloatV sqRadius = FMul(capsuleRadius, capsuleRadius);
//
//const bool overlap = status == GJK_CONTACT || FAllGrtrOrEq(sqRadius, sqDist);
//PX_PRINTF("CAPSULE status = %i, overlap = %i, PxVec3(%f, %f, %f)\n", status, overlap, capsGlobalPose.p.x, capsGlobalPose.p.y, capsGlobalPose.p.z);
return status == GJK_CONTACT || FAllGrtrOrEq(sqRadius, sqDist);
}
bool Gu::intersectSphereConvex(const Gu::Sphere& sphere, const Gu::ConvexMesh& mesh, const PxMeshScale& meshScale, const PxTransform& convexGlobalPose,
PxVec3*)
{
using namespace Ps::aos;
const Vec3V zeroV = V3Zero();
const Gu::ConvexHullData* hullData = &mesh.getHull();
const FloatV sphereRadius = FLoad(sphere.radius);
const Vec3V vScale = V3LoadU(meshScale.scale);
const QuatV vQuat = QuatVLoadU(&meshScale.rotation.x);
const PxTransform sphereTrans(sphere.center, PxQuat(PxIdentity));
const PsMatTransformV aToB(convexGlobalPose.transformInv(sphereTrans));
const Gu::ConvexHullV convexHull(hullData, zeroV, vScale, vQuat);
const Gu::CapsuleV capsule(aToB.p, sphereRadius);
//const Gu::CapsuleV capsule(zeroV, sphereRadius);
Vec3V contactA, contactB, normal;
FloatV sqDist;
PxGJKStatus status = Gu::GJKLocal(capsule, convexHull, contactA, contactB, normal, sqDist);
const FloatV sqRadius = FMul(sphereRadius, sphereRadius);
return status == GJK_CONTACT || FAllGrtrOrEq(sqRadius, sqDist) ;
}
static PxMat44 convertViewMatrix(const PxTransform& eye)
{
PxTransform viewMatrix = eye.getInverse();
PxMat44 mat44 = PxMat44(viewMatrix).getTranspose();
float m[16];
memcpy(m, mat44.front(), sizeof m);
PxMat44 view44;
view44.column0.x = m[0];
view44.column0.y = m[1];
view44.column0.z = m[2];
view44.column0.w = m[3];
view44.column1.x = m[4];
view44.column1.y = m[5];
view44.column1.z = m[6];
view44.column1.w = m[7];
view44.column2.x = m[8];
view44.column2.y = m[9];
view44.column2.z = m[10];
view44.column2.w = m[11];
view44.column3.x = m[12];
view44.column3.y = m[13];
view44.column3.z = m[14];
view44.column3.w = m[15];
PxMat44 tmpmat = view44.getTranspose(); view44 = tmpmat;
return view44;
}
