s444441/Space-Project
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
b7731e7faf
Space-Project/dependencies/physx-4.1/source/physxcharacterkinematic/src/CctCharacterController.cpp
xkamikoo 0446a2bfe3 new project
2021-01-16 17:11:39 +01:00

2555 lines
92 KiB
C++
Raw Blame History

//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of NVIDIA CORPORATION nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2019 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
#include "common/PxProfileZone.h"
#include "geometry/PxMeshQuery.h"
#include "PxRigidDynamic.h"
#include "CctCharacterController.h"
#include "CctCharacterControllerManager.h"
#include "CctSweptBox.h"
#include "CctSweptCapsule.h"
#include "CctObstacleContext.h"
#include "CmRenderOutput.h"
#include "GuIntersectionBoxBox.h"
#include "GuDistanceSegmentBox.h"
#include "PsMathUtils.h"
#include "PsFPU.h"
// PT: TODO: remove those includes.... shouldn't be allowed from here
#include "characterkinematic/PxControllerObstacles.h" // (*)
#include "characterkinematic/PxControllerManager.h" // (*)
#include "characterkinematic/PxControllerBehavior.h" // (*)
#include "CctInternalStructs.h" // (*)
//#define DEBUG_MTD
#ifdef DEBUG_MTD
#include <stdio.h>
#endif
#define MAX_ITER 10
using namespace physx;
using namespace Cct;
using namespace Gu;
using namespace Cm;
static const PxU32 gObstacleDebugColor = PxU32(PxDebugColor::eARGB_CYAN);
//static const PxU32 gCCTBoxDebugColor = PxU32(PxDebugColor::eARGB_YELLOW);
static const PxU32 gTBVDebugColor = PxU32(PxDebugColor::eARGB_MAGENTA);
static const bool gUsePartialUpdates = true;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
static PX_FORCE_INLINE PxHitFlags getSweepHitFlags(const CCTParams& params)
{
PxHitFlags sweepHitFlags = PxHitFlag::eDEFAULT/*|PxHitFlag::eMESH_BOTH_SIDES*/;
// sweepHitFlags |= PxHitFlag::eASSUME_NO_INITIAL_OVERLAP;
if(params.mPreciseSweeps)
sweepHitFlags |= PxHitFlag::ePRECISE_SWEEP;
return sweepHitFlags;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
static bool shouldApplyRecoveryModule(const PxRigidActor& rigidActor)
{
// PT: we must let the dynamic objects go through the CCT for proper 2-way interactions.
// But we should still apply the recovery module for kinematics.
const PxType type = rigidActor.getConcreteType();
if(type==PxConcreteType::eRIGID_STATIC)
return true;
if(type!=PxConcreteType::eRIGID_DYNAMIC)
return false;
return static_cast<const PxRigidBody&>(rigidActor).getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
static const bool gUseLocalSpace = true;
static PxVec3 worldToLocal(const PxObstacle& obstacle, const PxExtendedVec3& worldPos)
{
const PxTransform tr(toVec3(obstacle.mPos), obstacle.mRot);
return tr.transformInv(toVec3(worldPos));
}
static PxVec3 localToWorld(const PxObstacle& obstacle, const PxVec3& localPos)
{
const PxTransform tr(toVec3(obstacle.mPos), obstacle.mRot);
return tr.transform(localPos);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#ifdef PX_BIG_WORLDS
typedef PxExtendedBounds3 PxCCTBounds3;
typedef PxExtendedVec3 PxCCTVec3;
#else
typedef PxBounds3 PxCCTBounds3;
typedef PxVec3 PxCCTVec3;
#endif
static PX_INLINE void scale(PxCCTBounds3& b, const PxVec3& scale)
{
PxCCTVec3 center; getCenter(b, center);
PxVec3 extents; getExtents(b, extents);
extents.x *= scale.x;
extents.y *= scale.y;
extents.z *= scale.z;
setCenterExtents(b, center, extents);
}
static PX_INLINE void computeReflexionVector(PxVec3& reflected, const PxVec3& incomingDir, const PxVec3& outwardNormal)
{
reflected = incomingDir - outwardNormal * 2.0f * (incomingDir.dot(outwardNormal));
}
static PX_INLINE void collisionResponse(PxExtendedVec3& targetPosition, const PxExtendedVec3& currentPosition, const PxVec3& currentDir, const PxVec3& hitNormal, PxF32 bump, PxF32 friction, bool normalize=false)
{
// Compute reflect direction
PxVec3 reflectDir;
computeReflexionVector(reflectDir, currentDir, hitNormal);
reflectDir.normalize();
// Decompose it
PxVec3 normalCompo, tangentCompo;
Ps::decomposeVector(normalCompo, tangentCompo, reflectDir, hitNormal);
// Compute new destination position
const PxF32 amplitude = (targetPosition - currentPosition).magnitude();
targetPosition = currentPosition;
if(bump!=0.0f)
{
if(normalize)
normalCompo.normalize();
targetPosition += normalCompo*bump*amplitude;
}
if(friction!=0.0f)
{
if(normalize)
tangentCompo.normalize();
targetPosition += tangentCompo*friction*amplitude;
}
}
static PX_INLINE void relocateBox(PxBoxGeometry& boxGeom, PxTransform& pose, const PxExtendedVec3& center, const PxVec3& extents, const PxExtendedVec3& origin, const PxQuat& quatFromUp)
{
boxGeom.halfExtents = extents;
pose.p.x = float(center.x - origin.x);
pose.p.y = float(center.y - origin.y);
pose.p.z = float(center.z - origin.z);
pose.q = quatFromUp;
}
static PX_INLINE void relocateBox(PxBoxGeometry& boxGeom, PxTransform& pose, const TouchedUserBox& userBox)
{
relocateBox(boxGeom, pose, userBox.mBox.center, userBox.mBox.extents, userBox.mOffset, userBox.mBox.rot);
}
static PX_INLINE void relocateBox(PxBoxGeometry& boxGeom, PxTransform& pose, const TouchedBox& box)
{
boxGeom.halfExtents = box.mExtents;
pose.p = box.mCenter;
pose.q = box.mRot;
}
static PX_INLINE void relocateCapsule(
PxCapsuleGeometry& capsuleGeom, PxTransform& pose, const SweptCapsule* sc,
const PxQuat& quatFromUp,
const PxExtendedVec3& center, const PxExtendedVec3& origin)
{
capsuleGeom.radius = sc->mRadius;
capsuleGeom.halfHeight = 0.5f * sc->mHeight;
pose.p.x = float(center.x - origin.x);
pose.p.y = float(center.y - origin.y);
pose.p.z = float(center.z - origin.z);
pose.q = quatFromUp;
}
static PX_INLINE void relocateCapsule(PxCapsuleGeometry& capsuleGeom, PxTransform& pose, const PxVec3& p0, const PxVec3& p1, PxReal radius)
{
capsuleGeom.radius = radius;
pose = PxTransformFromSegment(p0, p1, &capsuleGeom.halfHeight);
if(capsuleGeom.halfHeight==0.0f)
capsuleGeom.halfHeight = FLT_EPSILON;
}
static PX_INLINE void relocateCapsule(PxCapsuleGeometry& capsuleGeom, PxTransform& pose, const TouchedUserCapsule& userCapsule)
{
PxVec3 p0, p1;
p0.x = float(userCapsule.mCapsule.p0.x - userCapsule.mOffset.x);
p0.y = float(userCapsule.mCapsule.p0.y - userCapsule.mOffset.y);
p0.z = float(userCapsule.mCapsule.p0.z - userCapsule.mOffset.z);
p1.x = float(userCapsule.mCapsule.p1.x - userCapsule.mOffset.x);
p1.y = float(userCapsule.mCapsule.p1.y - userCapsule.mOffset.y);
p1.z = float(userCapsule.mCapsule.p1.z - userCapsule.mOffset.z);
relocateCapsule(capsuleGeom, pose, p0, p1, userCapsule.mCapsule.radius);
}
static bool SweepBoxUserBox(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eBOX);
PX_ASSERT(geom->mType==TouchedGeomType::eUSER_BOX);
const SweptBox* SB = static_cast<const SweptBox*>(volume);
const TouchedUserBox* TC = static_cast<const TouchedUserBox*>(geom);
PxBoxGeometry boxGeom0;
PxTransform boxPose0;
// To precompute
relocateBox(boxGeom0, boxPose0, center, SB->mExtents, TC->mOffset, test->mUserParams.mQuatFromUp);
PxBoxGeometry boxGeom1;
PxTransform boxPose1;
relocateBox(boxGeom1, boxPose1, *TC);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, boxGeom0, boxPose0, boxGeom1, boxPose1, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mWorldNormal = sweepHit.normal;
impact.mDistance = sweepHit.distance;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
impact.setWorldPos(sweepHit.position, TC->mOffset);
return true;
}
static bool SweepBoxUserCapsule(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eBOX);
PX_ASSERT(geom->mType==TouchedGeomType::eUSER_CAPSULE);
const SweptBox* SB = static_cast<const SweptBox*>(volume);
const TouchedUserCapsule* TC = static_cast<const TouchedUserCapsule*>(geom);
PxBoxGeometry boxGeom;
PxTransform boxPose;
// To precompute
relocateBox(boxGeom, boxPose, center, SB->mExtents, TC->mOffset, test->mUserParams.mQuatFromUp);
PxCapsuleGeometry capsuleGeom;
PxTransform capsulePose;
relocateCapsule(capsuleGeom, capsulePose, *TC);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, boxGeom, boxPose, capsuleGeom, capsulePose, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
//TO CHECK: Investigate whether any significant performance improvement can be achieved through
// making the impact point computation optional in the sweep calls and compute it later
/*{
// ### check this
float t;
PxVec3 p;
float d = gUtilLib->PxSegmentOBBSqrDist(Capsule, Box0.center, Box0.extents, Box0.rot, &t, &p);
Box0.rot.multiply(p,p);
impact.mWorldPos.x = p.x + Box0.center.x + TC->mOffset.x;
impact.mWorldPos.y = p.y + Box0.center.y + TC->mOffset.y;
impact.mWorldPos.z = p.z + Box0.center.z + TC->mOffset.z;
}*/
{
impact.setWorldPos(sweepHit.position, TC->mOffset);
}
return true;
}
static bool sweepVolumeVsMesh( const SweepTest* sweepTest, const TouchedMesh* touchedMesh, SweptContact& impact,
const PxVec3& unitDir, const PxGeometry& geom, const PxTransform& pose,
PxU32 nbTris, const PxTriangle* triangles,
PxU32 cachedIndex)
{
PxSweepHit sweepHit;
if(PxMeshQuery::sweep(unitDir, impact.mDistance, geom, pose, nbTris, triangles, sweepHit, getSweepHitFlags(sweepTest->mUserParams), &cachedIndex))
{
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.setWorldPos(sweepHit.position, touchedMesh->mOffset);
// Returned index is only between 0 and nbTris, i.e. it indexes the array of cached triangles, not the original mesh.
PX_ASSERT(sweepHit.faceIndex < nbTris);
sweepTest->mCachedTriIndex[sweepTest->mCachedTriIndexIndex] = sweepHit.faceIndex;
// The CCT loop will use the index from the start of the cache...
impact.mInternalIndex = sweepHit.faceIndex + touchedMesh->mIndexWorldTriangles;
const PxU32* triangleIndices = &sweepTest->mTriangleIndices[touchedMesh->mIndexWorldTriangles];
impact.mTriangleIndex = triangleIndices[sweepHit.faceIndex];
return true;
}
return false;
}
static bool SweepBoxMesh(const SweepTest* sweep_test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eBOX);
PX_ASSERT(geom->mType==TouchedGeomType::eMESH);
const SweptBox* SB = static_cast<const SweptBox*>(volume);
const TouchedMesh* TM = static_cast<const TouchedMesh*>(geom);
PxU32 nbTris = TM->mNbTris;
if(!nbTris)
return false;
// Fetch triangle data for current mesh (the stream may contain triangles from multiple meshes)
const PxTriangle* T = &sweep_test->mWorldTriangles.getTriangle(TM->mIndexWorldTriangles);
// PT: this only really works when the CCT collides with a single mesh, but that's the most common case. When it doesn't, there's just no speedup but it still works.
PxU32 CachedIndex = sweep_test->mCachedTriIndex[sweep_test->mCachedTriIndexIndex];
if(CachedIndex>=nbTris)
CachedIndex=0;
PxBoxGeometry boxGeom;
boxGeom.halfExtents = SB->mExtents;
PxTransform boxPose(PxVec3(float(center.x - TM->mOffset.x), float(center.y - TM->mOffset.y), float(center.z - TM->mOffset.z)), sweep_test->mUserParams.mQuatFromUp); // Precompute
return sweepVolumeVsMesh(sweep_test, TM, impact, dir, boxGeom, boxPose, nbTris, T, CachedIndex);
}
static bool SweepCapsuleMesh(
const SweepTest* sweep_test, const SweptVolume* volume, const TouchedGeom* geom,
const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eCAPSULE);
PX_ASSERT(geom->mType==TouchedGeomType::eMESH);
const SweptCapsule* SC = static_cast<const SweptCapsule*>(volume);
const TouchedMesh* TM = static_cast<const TouchedMesh*>(geom);
PxU32 nbTris = TM->mNbTris;
if(!nbTris)
return false;
// Fetch triangle data for current mesh (the stream may contain triangles from multiple meshes)
const PxTriangle* T = &sweep_test->mWorldTriangles.getTriangle(TM->mIndexWorldTriangles);
// PT: this only really works when the CCT collides with a single mesh, but that's the most common case.
// When it doesn't, there's just no speedup but it still works.
PxU32 CachedIndex = sweep_test->mCachedTriIndex[sweep_test->mCachedTriIndexIndex];
if(CachedIndex>=nbTris)
CachedIndex=0;
PxCapsuleGeometry capsuleGeom;
PxTransform capsulePose;
relocateCapsule(capsuleGeom, capsulePose, SC, sweep_test->mUserParams.mQuatFromUp, center, TM->mOffset);
return sweepVolumeVsMesh(sweep_test, TM, impact, dir, capsuleGeom, capsulePose, nbTris, T, CachedIndex);
}
static bool SweepBoxBox(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eBOX);
PX_ASSERT(geom->mType==TouchedGeomType::eBOX);
const SweptBox* SB = static_cast<const SweptBox*>(volume);
const TouchedBox* TB = static_cast<const TouchedBox*>(geom);
PxBoxGeometry boxGeom0;
PxTransform boxPose0;
// To precompute
relocateBox(boxGeom0, boxPose0, center, SB->mExtents, TB->mOffset, test->mUserParams.mQuatFromUp);
PxBoxGeometry boxGeom1;
PxTransform boxPose1;
relocateBox(boxGeom1, boxPose1, *TB);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, boxGeom0, boxPose0, boxGeom1, boxPose1, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mWorldNormal = sweepHit.normal;
impact.mDistance = sweepHit.distance;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
impact.setWorldPos(sweepHit.position, TB->mOffset);
return true;
}
static bool SweepBoxSphere(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eBOX);
PX_ASSERT(geom->mType==TouchedGeomType::eSPHERE);
const SweptBox* SB = static_cast<const SweptBox*>(volume);
const TouchedSphere* TS = static_cast<const TouchedSphere*>(geom);
PxBoxGeometry boxGeom;
PxTransform boxPose;
// To precompute
relocateBox(boxGeom, boxPose, center, SB->mExtents, TS->mOffset, test->mUserParams.mQuatFromUp);
PxSphereGeometry sphereGeom;
sphereGeom.radius = TS->mRadius;
PxTransform spherePose;
spherePose.p = TS->mCenter;
spherePose.q = PxQuat(PxIdentity);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, boxGeom, boxPose, sphereGeom, spherePose, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
//TO CHECK: Investigate whether any significant performance improvement can be achieved through
// making the impact point computation optional in the sweep calls and compute it later
/*
{
// The sweep test doesn't compute the impact point automatically, so we have to do it here.
PxVec3 NewSphereCenter = TS->mSphere.center - d * dir;
PxVec3 Closest;
gUtilLib->PxPointOBBSqrDist(NewSphereCenter, Box0.center, Box0.extents, Box0.rot, &Closest);
// Compute point on the box, after sweep
Box0.rot.multiply(Closest, Closest);
impact.mWorldPos.x = TS->mOffset.x + Closest.x + Box0.center.x + d * dir.x;
impact.mWorldPos.y = TS->mOffset.y + Closest.y + Box0.center.y + d * dir.y;
impact.mWorldPos.z = TS->mOffset.z + Closest.z + Box0.center.z + d * dir.z;
impact.mWorldNormal = -impact.mWorldNormal;
}*/
{
impact.setWorldPos(sweepHit.position, TS->mOffset);
}
return true;
}
static bool SweepBoxCapsule(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eBOX);
PX_ASSERT(geom->mType==TouchedGeomType::eCAPSULE);
const SweptBox* SB = static_cast<const SweptBox*>(volume);
const TouchedCapsule* TC = static_cast<const TouchedCapsule*>(geom);
PxBoxGeometry boxGeom;
PxTransform boxPose;
// To precompute
relocateBox(boxGeom, boxPose, center, SB->mExtents, TC->mOffset, test->mUserParams.mQuatFromUp);
PxCapsuleGeometry capsuleGeom;
PxTransform capsulePose;
relocateCapsule(capsuleGeom, capsulePose, TC->mP0, TC->mP1, TC->mRadius);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, boxGeom, boxPose, capsuleGeom, capsulePose, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
//TO CHECK: Investigate whether any significant performance improvement can be achieved through
// making the impact point computation optional in the sweep calls and compute it later
/*{
float t;
PxVec3 p;
float d = gUtilLib->PxSegmentOBBSqrDist(TC->mCapsule, Box0.center, Box0.extents, Box0.rot, &t, &p);
Box0.rot.multiply(p,p);
impact.mWorldPos.x = p.x + Box0.center.x + TC->mOffset.x;
impact.mWorldPos.y = p.y + Box0.center.y + TC->mOffset.y;
impact.mWorldPos.z = p.z + Box0.center.z + TC->mOffset.z;
}*/
{
impact.setWorldPos(sweepHit.position, TC->mOffset);
}
return true;
}
static bool SweepCapsuleBox(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eCAPSULE);
PX_ASSERT(geom->mType==TouchedGeomType::eBOX);
const SweptCapsule* SC = static_cast<const SweptCapsule*>(volume);
const TouchedBox* TB = static_cast<const TouchedBox*>(geom);
PxCapsuleGeometry capsuleGeom;
PxTransform capsulePose;
relocateCapsule(capsuleGeom, capsulePose, SC, test->mUserParams.mQuatFromUp, center, TB->mOffset);
PxBoxGeometry boxGeom;
PxTransform boxPose;
// To precompute
relocateBox(boxGeom, boxPose, *TB);
// The box and capsule coordinates are relative to the center of the cached bounding box
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, capsuleGeom, capsulePose, boxGeom, boxPose, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
//TO CHECK: Investigate whether any significant performance improvement can be achieved through
// making the impact point computation optional in the sweep calls and compute it later
/*{
float t;
PxVec3 p;
float d = gUtilLib->PxSegmentOBBSqrDist(Capsule, TB->mBox.center, TB->mBox.extents, TB->mBox.rot, &t, &p);
TB->mBox.rot.multiply(p,p);
p += TB->mBox.center;
impact.mWorldPos.x = p.x + TB->mOffset.x;
impact.mWorldPos.y = p.y + TB->mOffset.y;
impact.mWorldPos.z = p.z + TB->mOffset.z;
}*/
{
impact.setWorldPos(sweepHit.position, TB->mOffset);
}
return true;
}
static bool SweepCapsuleSphere(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eCAPSULE);
PX_ASSERT(geom->mType==TouchedGeomType::eSPHERE);
const SweptCapsule* SC = static_cast<const SweptCapsule*>(volume);
const TouchedSphere* TS = static_cast<const TouchedSphere*>(geom);
PxCapsuleGeometry capsuleGeom;
PxTransform capsulePose;
relocateCapsule(capsuleGeom, capsulePose, SC, test->mUserParams.mQuatFromUp, center, TS->mOffset);
PxSphereGeometry sphereGeom;
sphereGeom.radius = TS->mRadius;
PxTransform spherePose;
spherePose.p = TS->mCenter;
spherePose.q = PxQuat(PxIdentity);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, capsuleGeom, capsulePose, sphereGeom, spherePose, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
impact.setWorldPos(sweepHit.position, TS->mOffset);
return true;
}
static bool SweepCapsuleCapsule(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eCAPSULE);
PX_ASSERT(geom->mType==TouchedGeomType::eCAPSULE);
const SweptCapsule* SC = static_cast<const SweptCapsule*>(volume);
const TouchedCapsule* TC = static_cast<const TouchedCapsule*>(geom);
PxCapsuleGeometry capsuleGeom0;
PxTransform capsulePose0;
relocateCapsule(capsuleGeom0, capsulePose0, SC, test->mUserParams.mQuatFromUp, center, TC->mOffset);
PxCapsuleGeometry capsuleGeom1;
PxTransform capsulePose1;
relocateCapsule(capsuleGeom1, capsulePose1, TC->mP0, TC->mP1, TC->mRadius);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, capsuleGeom0, capsulePose0, capsuleGeom1, capsulePose1, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
impact.setWorldPos(sweepHit.position, TC->mOffset);
return true;
}
static bool SweepCapsuleUserCapsule(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eCAPSULE);
PX_ASSERT(geom->mType==TouchedGeomType::eUSER_CAPSULE);
const SweptCapsule* SC = static_cast<const SweptCapsule*>(volume);
const TouchedUserCapsule* TC = static_cast<const TouchedUserCapsule*>(geom);
PxCapsuleGeometry capsuleGeom0;
PxTransform capsulePose0;
relocateCapsule(capsuleGeom0, capsulePose0, SC, test->mUserParams.mQuatFromUp, center, TC->mOffset);
PxCapsuleGeometry capsuleGeom1;
PxTransform capsulePose1;
relocateCapsule(capsuleGeom1, capsulePose1, *TC);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, capsuleGeom0, capsulePose0, capsuleGeom1, capsulePose1, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
impact.setWorldPos(sweepHit.position, TC->mOffset);
return true;
}
static bool SweepCapsuleUserBox(const SweepTest* test, const SweptVolume* volume, const TouchedGeom* geom, const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact)
{
PX_ASSERT(volume->getType()==SweptVolumeType::eCAPSULE);
PX_ASSERT(geom->mType==TouchedGeomType::eUSER_BOX);
const SweptCapsule* SC = static_cast<const SweptCapsule*>(volume);
const TouchedUserBox* TB = static_cast<const TouchedUserBox*>(geom);
PxCapsuleGeometry capsuleGeom;
PxTransform capsulePose;
relocateCapsule(capsuleGeom, capsulePose, SC, test->mUserParams.mQuatFromUp, center, TB->mOffset);
PxBoxGeometry boxGeom;
PxTransform boxPose;
relocateBox(boxGeom, boxPose, *TB);
PxSweepHit sweepHit;
if(!PxGeometryQuery::sweep(dir, impact.mDistance, capsuleGeom, capsulePose, boxGeom, boxPose, sweepHit, getSweepHitFlags(test->mUserParams)))
return false;
if(sweepHit.distance >= impact.mDistance)
return false;
impact.mDistance = sweepHit.distance;
impact.mWorldNormal = sweepHit.normal;
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
//TO CHECK: Investigate whether any significant performance improvement can be achieved through
// making the impact point computation optional in the sweep calls and compute it later
/*{
// ### check this
float t;
PxVec3 p;
float d = gUtilLib->PxSegmentOBBSqrDist(Capsule, Box.center, Box.extents, Box.rot, &t, &p);
p += Box.center;
impact.mWorldPos.x = p.x + TB->mOffset.x;
impact.mWorldPos.y = p.y + TB->mOffset.y;
impact.mWorldPos.z = p.z + TB->mOffset.z;
}*/
{
impact.setWorldPos(sweepHit.position, TB->mOffset);
}
return true;
}
typedef bool (*SweepFunc) (const SweepTest*, const SweptVolume*, const TouchedGeom*, const PxExtendedVec3&, const PxVec3&, SweptContact&);
static SweepFunc gSweepMap[SweptVolumeType::eLAST][TouchedGeomType::eLAST] = {
// Box funcs
{
SweepBoxUserBox,
SweepBoxUserCapsule,
SweepBoxMesh,
SweepBoxBox,
SweepBoxSphere,
SweepBoxCapsule
},
// Capsule funcs
{
SweepCapsuleUserBox,
SweepCapsuleUserCapsule,
SweepCapsuleMesh,
SweepCapsuleBox,
SweepCapsuleSphere,
SweepCapsuleCapsule
}
};
PX_COMPILE_TIME_ASSERT(sizeof(gSweepMap)==SweptVolumeType::eLAST*TouchedGeomType::eLAST*sizeof(SweepFunc));
static const PxU32 GeomSizes[] =
{
sizeof(TouchedUserBox),
sizeof(TouchedUserCapsule),
sizeof(TouchedMesh),
sizeof(TouchedBox),
sizeof(TouchedSphere),
sizeof(TouchedCapsule),
};
static const TouchedGeom* CollideGeoms(
const SweepTest* sweep_test, const SweptVolume& volume, const IntArray& geom_stream,
const PxExtendedVec3& center, const PxVec3& dir, SweptContact& impact, bool discardInitialOverlap)
{
impact.mInternalIndex = PX_INVALID_U32;
impact.mTriangleIndex = PX_INVALID_U32;
impact.mGeom = NULL;
const PxU32* Data = geom_stream.begin();
const PxU32* Last = geom_stream.end();
while(Data!=Last)
{
const TouchedGeom* CurrentGeom = reinterpret_cast<const TouchedGeom*>(Data);
SweepFunc ST = gSweepMap[volume.getType()][CurrentGeom->mType];
if(ST)
{
SweptContact C;
C.mDistance = impact.mDistance; // Initialize with current best distance
C.mInternalIndex = PX_INVALID_U32;
C.mTriangleIndex = PX_INVALID_U32;
if((ST)(sweep_test, &volume, CurrentGeom, center, dir, C))
{
if(C.mDistance==0.0f)
{
if(!discardInitialOverlap)
{
if(CurrentGeom->mType==TouchedGeomType::eUSER_BOX || CurrentGeom->mType==TouchedGeomType::eUSER_CAPSULE)
{
}
else
{
const PxRigidActor* touchedActor = CurrentGeom->mActor;
PX_ASSERT(touchedActor);
if(shouldApplyRecoveryModule(*touchedActor))
{
impact = C;
impact.mGeom = const_cast<TouchedGeom*>(CurrentGeom);
return CurrentGeom;
}
}
}
}
/* else
if(discardInitialOverlap && C.mDistance==0.0f)
{
// PT: we previously used eINITIAL_OVERLAP without eINITIAL_OVERLAP_KEEP, i.e. initially overlapping shapes got ignored.
// So we replicate this behavior here.
}*/
else if(C.mDistance<impact.mDistance)
{
impact = C;
impact.mGeom = const_cast<TouchedGeom*>(CurrentGeom);
if(C.mDistance <= 0.0f) // there is no point testing for closer hits
return CurrentGeom; // since we are touching a shape already
}
}
}
const PxU8* ptr = reinterpret_cast<const PxU8*>(Data);
ptr += GeomSizes[CurrentGeom->mType];
Data = reinterpret_cast<const PxU32*>(ptr);
}
return impact.mGeom;
}
static PxVec3 computeMTD(const SweepTest* sweep_test, const SweptVolume& volume, const IntArray& geom_stream, const PxExtendedVec3& center, float contactOffset)
{
PxVec3 p = toVec3(center);
// contactOffset += 0.01f;
const PxU32 maxIter = 4;
PxU32 nbIter = 0;
bool isValid = true;
while(isValid && nbIter<maxIter)
{
const PxU32* Data = geom_stream.begin();
const PxU32* Last = geom_stream.end();
while(Data!=Last)
{
const TouchedGeom* CurrentGeom = reinterpret_cast<const TouchedGeom*>(Data);
if(CurrentGeom->mType==TouchedGeomType::eUSER_BOX || CurrentGeom->mType==TouchedGeomType::eUSER_CAPSULE)
{
}
else
{
const PxRigidActor* touchedActor = CurrentGeom->mActor;
PX_ASSERT(touchedActor);
if(shouldApplyRecoveryModule(*touchedActor))
{
const PxShape* touchedShape = reinterpret_cast<const PxShape*>(CurrentGeom->mTGUserData);
PX_ASSERT(touchedShape);
const PxGeometryHolder gh = touchedShape->getGeometry();
const PxTransform globalPose = getShapeGlobalPose(*touchedShape, *touchedActor);
PxVec3 mtd;
PxF32 depth;
const PxTransform volumePose(p, sweep_test->mUserParams.mQuatFromUp);
if(volume.getType()==SweptVolumeType::eCAPSULE)
{
const SweptCapsule& sc = static_cast<const SweptCapsule&>(volume);
const PxCapsuleGeometry capsuleGeom(sc.mRadius+contactOffset, sc.mHeight*0.5f);
isValid = PxGeometryQuery::computePenetration(mtd, depth, capsuleGeom, volumePose, gh.any(), globalPose);
}
else
{
PX_ASSERT(volume.getType()==SweptVolumeType::eBOX);
const SweptBox& sb = static_cast<const SweptBox&>(volume);
const PxBoxGeometry boxGeom(sb.mExtents+PxVec3(contactOffset));
isValid = PxGeometryQuery::computePenetration(mtd, depth, boxGeom, volumePose, gh.any(), globalPose);
}
if(isValid)
{
nbIter++;
PX_ASSERT(depth>=0.0f);
PX_ASSERT(mtd.isFinite());
PX_ASSERT(PxIsFinite(depth));
#ifdef DEBUG_MTD
PX_ASSERT(depth<=1.0f);
if(depth>1.0f || !mtd.isFinite() || !PxIsFinite(depth))
{
int stop=1;
(void)stop;
}
printf("Depth: %f\n", depth);
printf("mtd: %f %f %f\n", mtd.x, mtd.y, mtd.z);
#endif
p += mtd * depth;
}
}
}
const PxU8* ptr = reinterpret_cast<const PxU8*>(Data);
ptr += GeomSizes[CurrentGeom->mType];
Data = reinterpret_cast<const PxU32*>(ptr);
}
}
return p;
}
static bool ParseGeomStream(const void* object, const IntArray& geom_stream)
{
const PxU32* Data = geom_stream.begin();
const PxU32* Last = geom_stream.end();
while(Data!=Last)
{
const TouchedGeom* CurrentGeom = reinterpret_cast<const TouchedGeom*>(Data);
if(CurrentGeom->mTGUserData==object)
return true;
const PxU8* ptr = reinterpret_cast<const PxU8*>(Data);
ptr += GeomSizes[CurrentGeom->mType];
Data = reinterpret_cast<const PxU32*>(ptr);
}
return false;
}
CCTParams::CCTParams() :
mNonWalkableMode (PxControllerNonWalkableMode::ePREVENT_CLIMBING),
mQuatFromUp (PxQuat(PxIdentity)),
mUpDirection (PxVec3(0.0f)),
mSlopeLimit (0.0f),
mContactOffset (0.0f),
mStepOffset (0.0f),
mInvisibleWallHeight (0.0f),
mMaxJumpHeight (0.0f),
mMaxEdgeLength2 (0.0f),
mTessellation (false),
mHandleSlope (false),
mOverlapRecovery (false),
mPreciseSweeps (true),
mPreventVerticalSlidingAgainstCeiling (false)
{
}
SweepTest::SweepTest(bool registerDeletionListener) :
mRenderBuffer (NULL),
mRenderFlags (0),
mTriangleIndices (PX_DEBUG_EXP("sweepTestTriangleIndices")),
mGeomStream (PX_DEBUG_EXP("sweepTestStream")),
mTouchedShape (registerDeletionListener),
mTouchedActor (registerDeletionListener),
mSQTimeStamp (0xffffffff),
mNbFullUpdates (0),
mNbPartialUpdates (0),
mNbTessellation (0),
mNbIterations (0),
mFlags (0),
mRegisterDeletionListener(registerDeletionListener),
mCctManager (NULL)
{
mCacheBounds.setEmpty();
mCachedTriIndexIndex = 0;
mCachedTriIndex[0] = mCachedTriIndex[1] = mCachedTriIndex[2] = 0;
mNbCachedStatic = 0;
mNbCachedT = 0;
mTouchedObstacleHandle = INVALID_OBSTACLE_HANDLE;
mTouchedPos = PxVec3(0);
mTouchedPosShape_Local = PxVec3(0);
mTouchedPosShape_World = PxVec3(0);
mTouchedPosObstacle_Local = PxVec3(0);
mTouchedPosObstacle_World = PxVec3(0);
// mVolumeGrowth = 1.2f; // Must be >1.0f and not too big
mVolumeGrowth = 1.5f; // Must be >1.0f and not too big
// mVolumeGrowth = 2.0f; // Must be >1.0f and not too big
mContactNormalDownPass = PxVec3(0.0f);
mContactNormalSidePass = PxVec3(0.0f);
mTouchedTriMin = 0.0f;
mTouchedTriMax = 0.0f;
}
SweepTest::~SweepTest()
{
// set the TouchedObject to NULL so we unregister the actor/shape
mTouchedShape = NULL;
mTouchedActor = NULL;
}
void SweepTest::voidTestCache()
{
mTouchedShape = NULL;
mTouchedActor = NULL;
mCacheBounds.setEmpty();
mTouchedObstacleHandle = INVALID_OBSTACLE_HANDLE;
}
void SweepTest::onRelease(const PxBase& observed)
{
if (mTouchedActor == &observed)
{
mTouchedShape = NULL;
mTouchedActor = NULL;
return;
}
if(ParseGeomStream(&observed, mGeomStream))
mCacheBounds.setEmpty();
if (mTouchedShape == &observed)
mTouchedShape = NULL;
}
void SweepTest::updateCachedShapesRegistration(PxU32 startIndex, bool unregister)
{
if(!mRegisterDeletionListener)
return;
if(!mGeomStream.size() || startIndex == mGeomStream.size())
return;
PX_ASSERT(startIndex <= mGeomStream.size());
const PxU32* data = &mGeomStream[startIndex];
const PxU32* last = mGeomStream.end();
while (data != last)
{
const TouchedGeom* CurrentGeom = reinterpret_cast<const TouchedGeom*>(data);
if (CurrentGeom->mActor)
{
if(unregister)
mCctManager->unregisterObservedObject(reinterpret_cast<const PxBase*>(CurrentGeom->mTGUserData));
else
mCctManager->registerObservedObject(reinterpret_cast<const PxBase*>(CurrentGeom->mTGUserData));
}
else
{
// we can early exit, the rest of the data are user obstacles
return;
}
const PxU8* ptr = reinterpret_cast<const PxU8*>(data);
ptr += GeomSizes[CurrentGeom->mType];
data = reinterpret_cast<const PxU32*>(ptr);
}
}
void SweepTest::onObstacleAdded(ObstacleHandle index, const PxObstacleContext* context, const PxVec3& origin, const PxVec3& unitDir, const PxReal distance )
{
if(mTouchedObstacleHandle != INVALID_OBSTACLE_HANDLE)
{
// check if new obstacle is closer
const ObstacleContext* obstContext = static_cast<const ObstacleContext*> (context);
PxRaycastHit obstacleHit;
const PxObstacle* obst = obstContext->raycastSingle(obstacleHit,index,origin,unitDir,distance);
if(obst && (obstacleHit.position.dot(unitDir))<(mTouchedPosObstacle_World.dot(unitDir)))
{
PX_ASSERT(obstacleHit.distance<=distance);
mTouchedObstacleHandle = index;
if(!gUseLocalSpace)
{
mTouchedPos = toVec3(obst->mPos);
}
else
{
mTouchedPosObstacle_World = obstacleHit.position;
mTouchedPosObstacle_Local = worldToLocal(*obst, PxExtendedVec3(PxExtended(obstacleHit.position.x),PxExtended(obstacleHit.position.y),PxExtended(obstacleHit.position.z)));
}
}
}
}
void SweepTest::onObstacleRemoved(ObstacleHandle index)
{
if(index == mTouchedObstacleHandle)
{
mTouchedObstacleHandle = INVALID_OBSTACLE_HANDLE;
}
}
void SweepTest::onObstacleUpdated(ObstacleHandle index, const PxObstacleContext* context, const PxVec3& origin, const PxVec3& unitDir, const PxReal distance)
{
if(index == mTouchedObstacleHandle)
{
// check if updated obstacle is still closest
const ObstacleContext* obstContext = static_cast<const ObstacleContext*> (context);
PxRaycastHit obstacleHit;
ObstacleHandle closestHandle = INVALID_OBSTACLE_HANDLE;
const PxObstacle* obst = obstContext->raycastSingle(obstacleHit,origin,unitDir,distance,closestHandle);
if(mTouchedObstacleHandle == closestHandle)
return;
if(obst)
{
PX_ASSERT(obstacleHit.distance<=distance);
mTouchedObstacleHandle = closestHandle;
if(!gUseLocalSpace)
{
mTouchedPos = toVec3(obst->mPos);
}
else
{
mTouchedPosObstacle_World = obstacleHit.position;
mTouchedPosObstacle_Local = worldToLocal(*obst, PxExtendedVec3(PxExtended(obstacleHit.position.x),PxExtended(obstacleHit.position.y),PxExtended(obstacleHit.position.z)));
}
}
}
}
void SweepTest::onOriginShift(const PxVec3& shift)
{
mCacheBounds.minimum -= shift;
mCacheBounds.maximum -= shift;
if(mTouchedShape)
{
const PxRigidActor* rigidActor = mTouchedActor.get();
if(rigidActor->getConcreteType() != PxConcreteType::eRIGID_STATIC)
{
mTouchedPosShape_World -= shift;
}
}
else if (mTouchedObstacleHandle != INVALID_OBSTACLE_HANDLE)
{
if(!gUseLocalSpace)
{
mTouchedPos -= shift;
}
else
{
mTouchedPosObstacle_World -= shift;
}
}
// adjust cache
PxU32* data = mGeomStream.begin();
PxU32* last = mGeomStream.end();
while(data != last)
{
TouchedGeom* currentGeom = reinterpret_cast<TouchedGeom*>(data);
currentGeom->mOffset -= shift;
PxU8* ptr = reinterpret_cast<PxU8*>(data);
ptr += GeomSizes[currentGeom->mType];
data = reinterpret_cast<PxU32*>(ptr);
}
}
static PxBounds3 getBounds3(const PxExtendedBounds3& extended)
{
return PxBounds3(toVec3(extended.minimum), toVec3(extended.maximum)); // LOSS OF ACCURACY
}
// PT: finds both touched CCTs and touched user-defined obstacles
void SweepTest::findTouchedObstacles(const UserObstacles& userObstacles, const PxExtendedBounds3& worldBox)
{
PxExtendedVec3 Origin; // Will be TouchedGeom::mOffset
getCenter(worldBox, Origin);
{
const PxU32 nbBoxes = userObstacles.mNbBoxes;
const PxExtendedBox* boxes = userObstacles.mBoxes;
const void** boxUserData = userObstacles.mBoxUserData;
const PxBounds3 singlePrecisionWorldBox = getBounds3(worldBox);
// Find touched boxes, i.e. other box controllers
for(PxU32 i=0;i<nbBoxes;i++)
{
const Gu::Box obb(
toVec3(boxes[i].center), // LOSS OF ACCURACY
boxes[i].extents,
PxMat33(boxes[i].rot)); // #### PT: TODO: useless conversion here
if(!Gu::intersectOBBAABB(obb, singlePrecisionWorldBox))
continue;
TouchedUserBox* UserBox = reinterpret_cast<TouchedUserBox*>(reserveContainerMemory(mGeomStream, sizeof(TouchedUserBox)/sizeof(PxU32)));
UserBox->mType = TouchedGeomType::eUSER_BOX;
UserBox->mTGUserData = boxUserData[i];
UserBox->mActor = NULL;
UserBox->mOffset = Origin;
UserBox->mBox = boxes[i];
}
}
{
// Find touched capsules, i.e. other capsule controllers
const PxU32 nbCapsules = userObstacles.mNbCapsules;
const PxExtendedCapsule* capsules = userObstacles.mCapsules;
const void** capsuleUserData = userObstacles.mCapsuleUserData;
PxExtendedVec3 Center;
PxVec3 Extents;
getCenter(worldBox, Center);
getExtents(worldBox, Extents);
for(PxU32 i=0;i<nbCapsules;i++)
{
// PT: do a quick AABB check first, to avoid calling the SDK too much
const PxF32 r = capsules[i].radius;
const PxExtended capMinx = PxMin(capsules[i].p0.x, capsules[i].p1.x);
const PxExtended capMaxx = PxMax(capsules[i].p0.x, capsules[i].p1.x);
if((capMinx - PxExtended(r) > worldBox.maximum.x) || (worldBox.minimum.x > capMaxx + PxExtended(r))) continue;
const PxExtended capMiny = PxMin(capsules[i].p0.y, capsules[i].p1.y);
const PxExtended capMaxy = PxMax(capsules[i].p0.y, capsules[i].p1.y);
if((capMiny - PxExtended(r) > worldBox.maximum.y) || (worldBox.minimum.y > capMaxy + PxExtended(r))) continue;
const PxExtended capMinz = PxMin(capsules[i].p0.z, capsules[i].p1.z);
const PxExtended capMaxz = PxMax(capsules[i].p0.z, capsules[i].p1.z);
if((capMinz - PxExtended(r) > worldBox.maximum.z) || (worldBox.minimum.z > capMaxz + PxExtended(r))) continue;
// PT: more accurate capsule-box test. Not strictly necessary but worth doing if available
const PxReal d2 = Gu::distanceSegmentBoxSquared(toVec3(capsules[i].p0), toVec3(capsules[i].p1), toVec3(Center), Extents, PxMat33(PxIdentity));
if(d2>r*r)
continue;
TouchedUserCapsule* UserCapsule = reinterpret_cast<TouchedUserCapsule*>(reserveContainerMemory(mGeomStream, sizeof(TouchedUserCapsule)/sizeof(PxU32)));
UserCapsule->mType = TouchedGeomType::eUSER_CAPSULE;
UserCapsule->mTGUserData = capsuleUserData[i];
UserCapsule->mActor = NULL;
UserCapsule->mOffset = Origin;
UserCapsule->mCapsule = capsules[i];
}
}
}
void SweepTest::updateTouchedGeoms( const InternalCBData_FindTouchedGeom* userData, const UserObstacles& userObstacles,
const PxExtendedBounds3& worldTemporalBox, const PxControllerFilters& filters, const PxVec3& sideVector)
{
/*
- if this is the first iteration (new frame) we have to redo the dynamic objects & the CCTs. The static objects can
be cached.
- if this is not, we can cache everything
*/
// PT: using "worldTemporalBox" instead of "mCacheBounds" seems to produce TTP 6207
//#define DYNAMIC_BOX worldTemporalBox
#define DYNAMIC_BOX mCacheBounds
bool newCachedBox = false;
CCTFilter filter;
filter.mFilterData = filters.mFilterData;
filter.mFilterCallback = filters.mFilterCallback;
filter.mPreFilter = filters.mFilterFlags & PxQueryFlag::ePREFILTER;
filter.mPostFilter = filters.mFilterFlags & PxQueryFlag::ePOSTFILTER;
// PT: detect changes to the static pruning structure
bool sceneHasChanged = false;
{
const PxU32 currentTimestamp = getSceneTimestamp(userData);
if(currentTimestamp!=mSQTimeStamp)
{
mSQTimeStamp = currentTimestamp;
sceneHasChanged = true;
}
}
// If the input box is inside the cached box, nothing to do
if(gUsePartialUpdates && !sceneHasChanged && worldTemporalBox.isInside(mCacheBounds))
{
//printf("CACHEIN%d\n", mFirstUpdate);
if(mFlags & STF_FIRST_UPDATE)
{
mFlags &= ~STF_FIRST_UPDATE;
// Only redo the dynamic
updateCachedShapesRegistration(mNbCachedStatic, true);
mGeomStream.forceSize_Unsafe(mNbCachedStatic);
mWorldTriangles.forceSize_Unsafe(mNbCachedT);
mTriangleIndices.forceSize_Unsafe(mNbCachedT);
filter.mStaticShapes = false;
if(filters.mFilterFlags & PxQueryFlag::eDYNAMIC)
filter.mDynamicShapes = true;
findTouchedGeometry(userData, DYNAMIC_BOX, mWorldTriangles, mTriangleIndices, mGeomStream, filter, mUserParams, mNbTessellation);
updateCachedShapesRegistration(mNbCachedStatic, false);
findTouchedObstacles(userObstacles, DYNAMIC_BOX);
mNbPartialUpdates++;
}
}
else
{
//printf("CACHEOUTNS=%d\n", mNbCachedStatic);
newCachedBox = true;
// Cache BV used for the query
mCacheBounds = worldTemporalBox;
// Grow the volume a bit. The temporal box here doesn't take sliding & collision response into account.
// In bad cases it is possible to eventually touch a portion of space not covered by this volume. Just
// in case, we grow the initial volume slightly. Then, additional tests are performed within the loop
// to make sure the TBV is always correct. There's a tradeoff between the original (artificial) growth
// of the volume, and the number of TBV recomputations performed at runtime...
scale(mCacheBounds, PxVec3(mVolumeGrowth));
// scale(mCacheBounds, PxVec3(mVolumeGrowth, 1.0f, mVolumeGrowth));
if(1 && !sideVector.isZero())
{
const PxVec3 sn = sideVector.getNormalized();
float dp0 = PxAbs((worldTemporalBox.maximum - worldTemporalBox.minimum).dot(sn));
float dp1 = PxAbs((mCacheBounds.maximum - mCacheBounds.minimum).dot(sn));
dp1 -= dp0;
dp1 *= 0.5f * 0.9f;
const PxVec3 offset = sn * dp1;
// printf("%f %f %f\n", offset.x, offset.y, offset.z);
mCacheBounds.minimum += offset;
mCacheBounds.maximum += offset;
add(mCacheBounds, worldTemporalBox);
PX_ASSERT(worldTemporalBox.isInside(mCacheBounds));
}
updateCachedShapesRegistration(0, true);
// Gather triangles touched by this box. This covers multiple meshes.
mWorldTriangles.clear();
mTriangleIndices.clear();
mGeomStream.clear();
// mWorldTriangles.reset();
// mTriangleIndices.reset();
// mGeomStream.reset();
mCachedTriIndexIndex = 0;
mCachedTriIndex[0] = mCachedTriIndex[1] = mCachedTriIndex[2] = 0;
mNbFullUpdates++;
if(filters.mFilterFlags & PxQueryFlag::eSTATIC)
filter.mStaticShapes = true;
filter.mDynamicShapes = false;
findTouchedGeometry(userData, mCacheBounds, mWorldTriangles, mTriangleIndices, mGeomStream, filter, mUserParams, mNbTessellation);
mNbCachedStatic = mGeomStream.size();
mNbCachedT = mWorldTriangles.size();
PX_ASSERT(mTriangleIndices.size()==mNbCachedT);
filter.mStaticShapes = false;
if(filters.mFilterFlags & PxQueryFlag::eDYNAMIC)
filter.mDynamicShapes = true;
findTouchedGeometry(userData, DYNAMIC_BOX, mWorldTriangles, mTriangleIndices, mGeomStream, filter, mUserParams, mNbTessellation);
// We can't early exit when no tris are touched since we also have to handle the boxes
updateCachedShapesRegistration(0, false);
findTouchedObstacles(userObstacles, DYNAMIC_BOX);
mFlags &= ~STF_FIRST_UPDATE;
//printf("CACHEOUTNSDONE=%d\n", mNbCachedStatic);
}
if(mRenderBuffer)
{
// PT: worldTemporalBox = temporal BV for this frame
RenderOutput out(*mRenderBuffer);
if(mRenderFlags & PxControllerDebugRenderFlag::eTEMPORAL_BV)
{
out << gTBVDebugColor;
out << DebugBox(getBounds3(worldTemporalBox));
}
if(mRenderFlags & PxControllerDebugRenderFlag::eCACHED_BV)
{
if(newCachedBox)
out << PxU32(PxDebugColor::eARGB_RED);
else
out << PxU32(PxDebugColor::eARGB_GREEN);
out << DebugBox(getBounds3(mCacheBounds));
}
}
}
// This is the generic sweep test for all swept volumes, but not character-controller specific
bool SweepTest::doSweepTest(const InternalCBData_FindTouchedGeom* userData,
InternalCBData_OnHit* userHitData,
const UserObstacles& userObstacles,
SweptVolume& swept_volume,
const PxVec3& direction, const PxVec3& sideVector, PxU32 max_iter, PxU32* nb_collisions,
float min_dist, const PxControllerFilters& filters, SweepPass sweepPass,
const PxRigidActor*& touchedActorOut, const PxShape*& touchedShapeOut, PxU64 contextID)
{
// Early exit when motion is zero. Since the motion is decomposed into several vectors
// and this function is called for each of them, it actually happens quite often.
if(direction.isZero())
return false;
PX_PROFILE_ZONE("CharacterController.doSweepTest", contextID);
PX_UNUSED(contextID);
bool hasMoved = false;
mFlags &= ~(STF_VALIDATE_TRIANGLE_DOWN|STF_TOUCH_OTHER_CCT|STF_TOUCH_OBSTACLE);
touchedShapeOut = NULL;
touchedActorOut = NULL;
mTouchedObstacleHandle = INVALID_OBSTACLE_HANDLE;
PxExtendedVec3 currentPosition = swept_volume.mCenter;
PxExtendedVec3 targetOrientation = swept_volume.mCenter;
targetOrientation += direction;
PxU32 NbCollisions = 0;
while(max_iter--)
{
mNbIterations++;
// Compute current direction
PxVec3 currentDirection = targetOrientation - currentPosition;
// Make sure the new TBV is still valid
{
// Compute temporal bounding box. We could use a capsule or an OBB instead:
// - the volume would be smaller
// - but the query would be slower
// Overall it's unclear whether it's worth it or not.
// TODO: optimize this part ?
PxExtendedBounds3 temporalBox;
swept_volume.computeTemporalBox(*this, temporalBox, currentPosition, currentDirection);
// Gather touched geoms
updateTouchedGeoms(userData, userObstacles, temporalBox, filters, sideVector);
}
const float Length = currentDirection.magnitude();
if(Length<=min_dist) //Use <= to handle the case where min_dist is zero.
break;
currentDirection /= Length;
// From Quake2: "if velocity is against the original velocity, stop dead to avoid tiny occilations in sloping corners"
if((currentDirection.dot(direction)) <= 0.0f)
break;
// From this point, we're going to update the position at least once
hasMoved = true;
// Find closest collision
SweptContact C;
C.mDistance = Length + mUserParams.mContactOffset;
if(!CollideGeoms(this, swept_volume, mGeomStream, currentPosition, currentDirection, C, !mUserParams.mOverlapRecovery))
{
// no collision found => move to desired position
currentPosition = targetOrientation;
break;
}
PX_ASSERT(C.mGeom); // If we reach this point, we must have touched a geom
if(mUserParams.mOverlapRecovery && C.mDistance==0.0f)
{
/* SweptContact C;
C.mDistance = 10.0f;
CollideGeoms(this, swept_volume, mGeomStream, currentPosition, -currentDirection, C, true);
currentPosition -= currentDirection*C.mDistance;
C.mDistance = 10.0f;
CollideGeoms(this, swept_volume, mGeomStream, currentPosition, currentDirection, C, true);
const float DynSkin = mUserParams.mContactOffset;
if(C.mDistance>DynSkin)
currentPosition += currentDirection*(C.mDistance-DynSkin);*/
const PxVec3 mtd = computeMTD(this, swept_volume, mGeomStream, currentPosition, mUserParams.mContactOffset);
NbCollisions++;
if(nb_collisions)
*nb_collisions = NbCollisions;
#ifdef DEBUG_MTD
printf("MTD FIXUP: %f %f %f\n", mtd.x - swept_volume.mCenter.x, mtd.y - swept_volume.mCenter.y, mtd.z - swept_volume.mCenter.z);
#endif
swept_volume.mCenter.x = PxExtended(mtd.x);
swept_volume.mCenter.y = PxExtended(mtd.y);
swept_volume.mCenter.z = PxExtended(mtd.z);
return hasMoved;
// currentPosition.x = mtd.x;
// currentPosition.y = mtd.y;
// currentPosition.z = mtd.z;
// continue;
}
bool preventVerticalMotion = false;
bool stopSliding = true;
if(C.mGeom->mType==TouchedGeomType::eUSER_BOX || C.mGeom->mType==TouchedGeomType::eUSER_CAPSULE)
{
if(sweepPass!=SWEEP_PASS_SENSOR)
{
// We touched a user object, typically another CCT, but can also be a user-defined obstacle
// PT: TODO: technically lines marked with (*) shouldn't be here... revisit later
const PxObstacle* touchedObstacle = NULL; // (*)
ObstacleHandle touchedObstacleHandle = INVALID_OBSTACLE_HANDLE;
// if(mValidateCallback)
{
PxInternalCBData_OnHit* internalData = static_cast<PxInternalCBData_OnHit*>(userHitData); // (*)
internalData->touchedObstacle = NULL; // (*)
internalData->touchedObstacleHandle = INVALID_OBSTACLE_HANDLE;
const PxU32 behaviorFlags = userHitCallback(userHitData, C, currentDirection, Length);
stopSliding = (behaviorFlags & PxControllerBehaviorFlag::eCCT_SLIDE)==0; // (*)
touchedObstacle = internalData->touchedObstacle; // (*)
touchedObstacleHandle = internalData->touchedObstacleHandle;
}
// printf("INTERNAL: %d\n", int(touchedObstacle));
if(sweepPass==SWEEP_PASS_DOWN)
{
// (*)
if(touchedObstacle)
{
mFlags |= STF_TOUCH_OBSTACLE;
mTouchedObstacleHandle = touchedObstacleHandle;
if(!gUseLocalSpace)
{
mTouchedPos = toVec3(touchedObstacle->mPos);
}
else
{
mTouchedPosObstacle_World = toVec3(C.mWorldPos);
mTouchedPosObstacle_Local = worldToLocal(*touchedObstacle, C.mWorldPos);
}
}
else
{
mFlags |= STF_TOUCH_OTHER_CCT;
}
}
}
}
else
{
const PxShape* touchedShape = reinterpret_cast<const PxShape*>(C.mGeom->mTGUserData);
PX_ASSERT(touchedShape);
const PxRigidActor* touchedActor = C.mGeom->mActor;
PX_ASSERT(touchedActor);
// We touched a normal object
if(sweepPass==SWEEP_PASS_DOWN)
{
mFlags &= ~(STF_TOUCH_OTHER_CCT|STF_TOUCH_OBSTACLE);
#ifdef USE_CONTACT_NORMAL_FOR_SLOPE_TEST
mFlags |= STF_VALIDATE_TRIANGLE_DOWN;
mContactNormalDownPass = C.mWorldNormal;
#else
// Work out if the shape is attached to a static or dynamic actor.
// The slope limit is currently only considered when walking on static actors.
// It is ignored for shapes attached attached to dynamics and kinematics.
// TODO: 1. should we treat stationary kinematics the same as statics.
// 2. should we treat all kinematics the same as statics.
// 3. should we treat no kinematics the same as statics.
if((touchedActor->getConcreteType() == PxConcreteType::eRIGID_STATIC) && (C.mInternalIndex!=PX_INVALID_U32))
{
mFlags |= STF_VALIDATE_TRIANGLE_DOWN;
const PxTriangle& touchedTri = mWorldTriangles.getTriangle(C.mInternalIndex);
const PxVec3& upDirection = mUserParams.mUpDirection;
const float dp0 = touchedTri.verts[0].dot(upDirection);
const float dp1 = touchedTri.verts[1].dot(upDirection);
const float dp2 = touchedTri.verts[2].dot(upDirection);
float dpmin = dp0;
dpmin = physx::intrinsics::selectMin(dpmin, dp1);
dpmin = physx::intrinsics::selectMin(dpmin, dp2);
float dpmax = dp0;
dpmax = physx::intrinsics::selectMax(dpmax, dp1);
dpmax = physx::intrinsics::selectMax(dpmax, dp2);
PxExtendedVec3 cacheCenter;
getCenter(mCacheBounds, cacheCenter);
const float offset = upDirection.dot(toVec3(cacheCenter));
mTouchedTriMin = dpmin + offset;
mTouchedTriMax = dpmax + offset;
touchedTri.normal(mContactNormalDownPass);
}
#endif
// Update touched shape in down pass
touchedShapeOut = const_cast<PxShape*>(touchedShape);
touchedActorOut = touchedActor;
// mTouchedPos = getShapeGlobalPose(*touchedShape).p;
const PxTransform shapeTransform = getShapeGlobalPose(*touchedShape, *touchedActor);
const PxVec3 worldPos = toVec3(C.mWorldPos);
mTouchedPosShape_World = worldPos;
mTouchedPosShape_Local = shapeTransform.transformInv(worldPos);
}
else if(sweepPass==SWEEP_PASS_SIDE || sweepPass==SWEEP_PASS_SENSOR)
{
if((touchedActor->getConcreteType() == PxConcreteType::eRIGID_STATIC) && (C.mInternalIndex!=PX_INVALID_U32))
{
mFlags |= STF_VALIDATE_TRIANGLE_SIDE;
const PxTriangle& touchedTri = mWorldTriangles.getTriangle(C.mInternalIndex);
touchedTri.normal(mContactNormalSidePass);
// printf("%f | %f | %f\n", mContactNormalSidePass.x, mContactNormalSidePass.y, mContactNormalSidePass.z);
if(mUserParams.mPreventVerticalSlidingAgainstCeiling && mContactNormalSidePass.dot(mUserParams.mUpDirection)<0.0f)
preventVerticalMotion = true;
}
}
if(sweepPass!=SWEEP_PASS_SENSOR)
// if(mValidateCallback)
{
const PxU32 behaviorFlags = shapeHitCallback(userHitData, C, currentDirection, Length);
stopSliding = (behaviorFlags & PxControllerBehaviorFlag::eCCT_SLIDE)==0; // (*)
}
}
if(sweepPass==SWEEP_PASS_DOWN && !stopSliding)
{
// Trying to solve the following problem:
// - by default, the CCT "friction" is infinite, i.e. a CCT will not slide on a slope (this is by design)
// - this produces bad results when a capsule CCT stands on top of another capsule CCT, without sliding. Visually it looks
// like the character is standing on the other character's head, it looks bad. So, here, we would like to let the CCT
// slide away, i.e. we don't want friction.
// So here we simply increase the number of iterations (== let the CCT slide) when the first down collision is with another CCT.
if(!NbCollisions)
max_iter += 9;
// max_iter += 1;
}
NbCollisions++;
// mContactPointHeight = (float)C.mWorldPos[mUserParams.mUpDirection]; // UBI
mContactPointHeight = toVec3(C.mWorldPos).dot(mUserParams.mUpDirection); // UBI
const float DynSkin = mUserParams.mContactOffset;
if(C.mDistance>DynSkin/*+0.01f*/)
currentPosition += currentDirection*(C.mDistance-DynSkin);
// DE6513
/* else if(sweepPass==SWEEP_PASS_SIDE)
{
// Might be better to do a proper sweep pass here, in the opposite direction
currentPosition += currentDirection*(C.mDistance-DynSkin);
}*/
//~DE6513
PxVec3 WorldNormal = C.mWorldNormal;
if(preventVerticalMotion || ((mFlags & STF_WALK_EXPERIMENT) && (mUserParams.mNonWalkableMode!=PxControllerNonWalkableMode::ePREVENT_CLIMBING_AND_FORCE_SLIDING)))
{
// Make sure the auto-step doesn't bypass this !
// PT: cancel out normal compo
// WorldNormal[mUserParams.mUpDirection]=0.0f;
// WorldNormal.normalize();
PxVec3 normalCompo, tangentCompo;
Ps::decomposeVector(normalCompo, tangentCompo, WorldNormal, mUserParams.mUpDirection);
WorldNormal = tangentCompo;
WorldNormal.normalize();
}
const float Bump = 0.0f; // ### doesn't work when !=0 because of Quake2 hack!
const float Friction = 1.0f;
collisionResponse(targetOrientation, currentPosition, currentDirection, WorldNormal, Bump, Friction, (mFlags & STF_NORMALIZE_RESPONSE)!=0);
}
if(nb_collisions)
*nb_collisions = NbCollisions;
// Final box position that should be reflected in the graphics engine
swept_volume.mCenter = currentPosition;
// If we didn't move, don't update the box position at all (keeping possible lazy-evaluated structures valid)
return hasMoved;
}
// ### have a return code to tell if we really moved or not
// Using swept code & direct position update (no physics engine)
// This function is the generic character controller logic, valid for all swept volumes
PxControllerCollisionFlags SweepTest::moveCharacter(
const InternalCBData_FindTouchedGeom* userData,
InternalCBData_OnHit* userHitData,
SweptVolume& volume,
const PxVec3& direction,
const UserObstacles& userObstacles,
float min_dist,
const PxControllerFilters& filters,
bool constrainedClimbingMode,
bool standingOnMoving,
const PxRigidActor*& touchedActor,
const PxShape*& touchedShape,
PxU64 contextID)
{
PX_PROFILE_ZONE("CharacterController.moveCharacter", contextID);
PX_UNUSED(contextID);
bool standingOnMovingUp = standingOnMoving;
mFlags &= ~STF_HIT_NON_WALKABLE;
PxControllerCollisionFlags CollisionFlags = PxControllerCollisionFlags(0);
const PxU32 maxIter = MAX_ITER; // 1 for "collide and stop"
const PxU32 maxIterSides = maxIter;
const PxU32 maxIterDown = ((mFlags & STF_WALK_EXPERIMENT) && mUserParams.mNonWalkableMode==PxControllerNonWalkableMode::ePREVENT_CLIMBING_AND_FORCE_SLIDING) ? maxIter : 1;
// const PxU32 maxIterDown = 1;
// ### this causes the artificial gap on top of chars
float stepOffset = mUserParams.mStepOffset; // Default step offset can be cancelled in some cases.
// Save initial height
const PxVec3& upDirection = mUserParams.mUpDirection;
const PxExtended originalHeight = volume.mCenter.dot(upDirection);
const PxExtended originalBottomPoint = originalHeight - PxExtended(volume.mHalfHeight); // UBI
// TEST! Disable auto-step when flying. Not sure this is really useful.
// if(direction[upDirection]>0.0f)
const float dir_dot_up = direction.dot(upDirection);
//printf("%f\n", dir_dot_up);
if(dir_dot_up>0.0f)
{
mFlags |= STF_IS_MOVING_UP;
// PT: this makes it fail on a platform moving up when jumping
// However if we don't do that a jump when moving up a slope doesn't work anymore!
// Not doing this also creates jittering when a capsule CCT jumps against another capsule CCT
if(!standingOnMovingUp) // PT: if we're standing on something moving up it's safer to do the up motion anyway, even though this won't work well before we add the flag in TA13542
{
// static int count=0; printf("Cancelling step offset... %d\n", count++);
stepOffset = 0.0f;
}
}
else
{
mFlags &= ~STF_IS_MOVING_UP;
}
// Decompose motion into 3 independent motions: up, side, down
// - if the motion is purely down (gravity only), the up part is needed to fight accuracy issues. For example if the
// character is already touching the geometry a bit, the down sweep test might have troubles. If we first move it above
// the geometry, the problems disappear.
// - if the motion is lateral (character moving forward under normal gravity) the decomposition provides the autostep feature
// - if the motion is purely up, the down part can be skipped
PxVec3 UpVector(0.0f, 0.0f, 0.0f);
PxVec3 DownVector(0.0f, 0.0f, 0.0f);
PxVec3 normal_compo, tangent_compo;
Ps::decomposeVector(normal_compo, tangent_compo, direction, upDirection);
// if(direction[upDirection]<0.0f)
if(dir_dot_up<=0.0f)
// DownVector[upDirection] = direction[upDirection];
DownVector = normal_compo;
else
// UpVector[upDirection] = direction[upDirection];
UpVector = normal_compo;
// PxVec3 SideVector = direction;
// SideVector[upDirection] = 0.0f;
PxVec3 SideVector = tangent_compo;
// If the side motion is zero, i.e. if the character is not really moving, disable auto-step.
// This is important to prevent the CCT from automatically climbing on small objects that move
// against it. We should climb over those only if there's a valid side motion from the player.
const bool sideVectorIsZero = !standingOnMovingUp && Ps::isAlmostZero(SideVector); // We can't use PxVec3::isZero() safely with arbitrary up vectors
// #### however if we do this the up pass is disabled, with bad consequences when the CCT is on a dynamic object!!
// ### this line makes it possible to push other CCTs by jumping on them
// const bool sideVectorIsZero = false;
// printf("sideVectorIsZero: %d\n", sideVectorIsZero);
// if(!SideVector.isZero())
if(!sideVectorIsZero)
// UpVector[upDirection] += stepOffset;
UpVector += upDirection*stepOffset;
// printf("stepOffset: %f\n", stepOffset);
// ==========[ Initial volume query ]===========================
// PT: the main difference between this initial query and subsequent ones is that we use the
// full direction vector here, not the components along each axis. So there is a good chance
// that this initial query will contain all the motion we need, and thus subsequent queries
// will be skipped.
{
PxExtendedBounds3 temporalBox;
volume.computeTemporalBox(*this, temporalBox, volume.mCenter, direction);
// Gather touched geoms
updateTouchedGeoms(userData, userObstacles, temporalBox, filters, SideVector);
}
// ==========[ UP PASS ]===========================
mCachedTriIndexIndex = 0;
const bool performUpPass = true;
PxU32 NbCollisions=0;
PxU32 maxIterUp;
if(mUserParams.mPreventVerticalSlidingAgainstCeiling)
maxIterUp = 1;
else
maxIterUp = Ps::isAlmostZero(SideVector) ? maxIter : 1;
if(performUpPass)
{
// printf("%f | %f | %f\n", UpVector.x, UpVector.y, UpVector.z);
// Prevent user callback for up motion. This up displacement is artificial, and only needed for auto-stepping.
// If we call the user for this, we might eventually apply upward forces to objects resting on top of us, even
// if we visually don't move. This produces weird-looking motions.
// mValidateCallback = false;
// PT: actually I think the previous comment is wrong. It's not only needed for auto-stepping: when the character
// jumps there's a legit up motion and the lack of callback in that case could need some object can't be pushed
// by the character's 'head' (for example). So I now think it's better to use the callback all the time, and
// let users figure out what to do using the available state (like "isMovingUp", etc).
// mValidateCallback = true;
// In the walk-experiment we explicitly want to ban any up motions, to avoid characters climbing slopes they shouldn't climb.
// So let's bypass the whole up pass.
if(!(mFlags & STF_WALK_EXPERIMENT))
{
// ### maxIter here seems to "solve" the V bug
if(doSweepTest(userData, userHitData, userObstacles, volume, UpVector, SideVector, maxIterUp, &NbCollisions, min_dist, filters, SWEEP_PASS_UP, touchedActor, touchedShape, contextID))
{
if(NbCollisions)
{
CollisionFlags |= PxControllerCollisionFlag::eCOLLISION_UP;
// Clamp step offset to make sure we don't undo more than what we did
float Delta = float(volume.mCenter.dot(upDirection) - originalHeight);
if(Delta<stepOffset)
{
stepOffset=Delta;
}
}
}
}
}
// ==========[ SIDE PASS ]===========================
mCachedTriIndexIndex = 1;
// mValidateCallback = true;
const bool PerformSidePass = true;
mFlags &= ~STF_VALIDATE_TRIANGLE_SIDE;
if(PerformSidePass)
{
NbCollisions=0;
//printf("BS:%.2f %.2f %.2f NS=%d\n", volume.mCenter.x, volume.mCenter.y, volume.mCenter.z, mNbCachedStatic);
if(doSweepTest(userData, userHitData, userObstacles, volume, SideVector, SideVector, maxIterSides, &NbCollisions, min_dist, filters, SWEEP_PASS_SIDE, touchedActor, touchedShape, contextID))
{
if(NbCollisions)
CollisionFlags |= PxControllerCollisionFlag::eCOLLISION_SIDES;
}
//printf("AS:%.2f %.2f %.2f NS=%d\n", volume.mCenter.x, volume.mCenter.y, volume.mCenter.z, mNbCachedStatic);
if(1 && constrainedClimbingMode && volume.getType()==SweptVolumeType::eCAPSULE && !(mFlags & STF_VALIDATE_TRIANGLE_SIDE))
{
const float capsuleRadius = static_cast<const SweptCapsule&>(volume).mRadius;
const float sideM = SideVector.magnitude();
if(sideM<capsuleRadius)
{
const PxVec3 sensor = SideVector.getNormalized() * capsuleRadius;
mFlags &= ~STF_VALIDATE_TRIANGLE_SIDE;
NbCollisions=0;
//printf("BS:%.2f %.2f %.2f NS=%d\n", volume.mCenter.x, volume.mCenter.y, volume.mCenter.z, mNbCachedStatic);
const PxExtendedVec3 saved = volume.mCenter;
doSweepTest(userData, userHitData, userObstacles, volume, sensor, SideVector, 1, &NbCollisions, min_dist, filters, SWEEP_PASS_SENSOR, touchedActor, touchedShape, contextID);
volume.mCenter = saved;
}
}
}
// ==========[ DOWN PASS ]===========================
mCachedTriIndexIndex = 2;
const bool PerformDownPass = true;
if(PerformDownPass)
{
NbCollisions=0;
// if(!SideVector.isZero()) // We disabled that before so we don't have to undo it in that case
if(!sideVectorIsZero) // We disabled that before so we don't have to undo it in that case
// DownVector[upDirection] -= stepOffset; // Undo our artificial up motion
DownVector -= upDirection*stepOffset; // Undo our artificial up motion
mFlags &= ~STF_VALIDATE_TRIANGLE_DOWN;
touchedShape = NULL;
touchedActor = NULL;
mTouchedObstacleHandle = INVALID_OBSTACLE_HANDLE;
// min_dist actually makes a big difference :(
// AAARRRGGH: if we get culled because of min_dist here, mValidateTriangle never becomes valid!
if(doSweepTest(userData, userHitData, userObstacles, volume, DownVector, SideVector, maxIterDown, &NbCollisions, min_dist, filters, SWEEP_PASS_DOWN, touchedActor, touchedShape, contextID))
{
if(NbCollisions)
{
if(dir_dot_up<=0.0f) // PT: fix attempt
CollisionFlags |= PxControllerCollisionFlag::eCOLLISION_DOWN;
if(mUserParams.mHandleSlope && !(mFlags & (STF_TOUCH_OTHER_CCT|STF_TOUCH_OBSTACLE))) // PT: I think the following fix shouldn't be performed when mHandleSlope is false.
{
// PT: the following code is responsible for a weird capsule behaviour,
// when colliding against a highly tesselated terrain:
// - with a large direction vector, the capsule gets stuck against some part of the terrain
// - with a slower direction vector (but in the same direction!) the capsule manages to move
// I will keep that code nonetheless, since it seems to be useful for them.
//printf("%d\n", mFlags & STF_VALIDATE_TRIANGLE_SIDE);
// constrainedClimbingMode
if((mFlags & STF_VALIDATE_TRIANGLE_SIDE) && testSlope(mContactNormalSidePass, upDirection, mUserParams.mSlopeLimit))
{
//printf("%d\n", mFlags & STF_VALIDATE_TRIANGLE_SIDE);
if(constrainedClimbingMode && PxExtended(mContactPointHeight) > originalBottomPoint + PxExtended(stepOffset))
{
mFlags |= STF_HIT_NON_WALKABLE;
if(!(mFlags & STF_WALK_EXPERIMENT))
return CollisionFlags;
// printf("Contrained\n");
}
}
//~constrainedClimbingMode
}
}
}
//printf("AD:%.2f %.2f %.2f NS=%d\n", volume.mCenter.x, volume.mCenter.y, volume.mCenter.z, mNbCachedStatic);
// printf("%d\n", mTouchOtherCCT);
// TEST: do another down pass if we're on a non-walkable poly
// ### kind of works but still not perfect
// ### could it be because we zero the Y impulse later?
// ### also check clamped response vectors
// if(mUserParams.mHandleSlope && mValidateTriangle && direction[upDirection]<0.0f)
// if(mUserParams.mHandleSlope && !mTouchOtherCCT && !mTouchObstacle && mValidateTriangle && dir_dot_up<0.0f)
if(mUserParams.mHandleSlope && !(mFlags & (STF_TOUCH_OTHER_CCT|STF_TOUCH_OBSTACLE)) && (mFlags & STF_VALIDATE_TRIANGLE_DOWN) && dir_dot_up<=0.0f)
{
PxVec3 Normal;
#ifdef USE_CONTACT_NORMAL_FOR_SLOPE_TEST
Normal = mContactNormalDownPass;
#else
//mTouchedTriangle.normal(Normal);
Normal = mContactNormalDownPass;
#endif
const float touchedTriHeight = float(PxExtended(mTouchedTriMax) - originalBottomPoint);
/* if(touchedTriHeight>mUserParams.mStepOffset)
{
if(constrainedClimbingMode && mContactPointHeight > originalBottomPoint + stepOffset)
{
mFlags |= STF_HIT_NON_WALKABLE;
if(!(mFlags & STF_WALK_EXPERIMENT))
return CollisionFlags;
}
}*/
if(touchedTriHeight>mUserParams.mStepOffset && testSlope(Normal, upDirection, mUserParams.mSlopeLimit))
{
mFlags |= STF_HIT_NON_WALKABLE;
// Early exit if we're going to run this again anyway...
if(!(mFlags & STF_WALK_EXPERIMENT))
return CollisionFlags;
/* CatchScene()->GetRenderer()->AddLine(mTouchedTriangle.mVerts[0], mTouched.mVerts[1], ARGB_YELLOW);
CatchScene()->GetRenderer()->AddLine(mTouchedTriangle.mVerts[0], mTouched.mVerts[2], ARGB_YELLOW);
CatchScene()->GetRenderer()->AddLine(mTouchedTriangle.mVerts[1], mTouched.mVerts[2], ARGB_YELLOW);
*/
// ==========[ WALK EXPERIMENT ]===========================
mFlags |= STF_NORMALIZE_RESPONSE;
const PxExtended tmp = volume.mCenter.dot(upDirection);
float Delta = tmp > originalHeight ? float(tmp - originalHeight) : 0.0f;
Delta += fabsf(direction.dot(upDirection));
float Recover = Delta;
NbCollisions=0;
const float MD = Recover < min_dist ? Recover/float(maxIter) : min_dist;
PxVec3 RecoverPoint(0,0,0);
RecoverPoint = -upDirection*Recover;
// PT: we pass "SWEEP_PASS_UP" for compatibility with previous code, but it's technically wrong (this is a 'down' pass)
if(doSweepTest(userData, userHitData, userObstacles, volume, RecoverPoint, SideVector, maxIter, &NbCollisions, MD, filters, SWEEP_PASS_UP, touchedActor, touchedShape, contextID))
{
// if(NbCollisions) CollisionFlags |= COLLISION_Y_DOWN;
// PT: why did we do this ? Removed for now. It creates a bug (non registered event) when we land on a steep poly.
// However this might have been needed when we were sliding on those polygons, and we didn't want the land anim to
// start while we were sliding.
// if(NbCollisions) CollisionFlags &= ~PxControllerCollisionFlag::eCOLLISION_DOWN;
}
mFlags &= ~STF_NORMALIZE_RESPONSE;
}
}
}
return CollisionFlags;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// This is an interface between NX users and the internal character controller module.
#include "characterkinematic/PxControllerBehavior.h"
#include "PxActor.h"
#include "PxScene.h"
#include "CctInternalStructs.h"
#include "CctBoxController.h"
#include "CctCapsuleController.h"
#include "CctCharacterControllerManager.h"
#include "CctObstacleContext.h"
// PT: we use a local class instead of making "Controller" a PxQueryFilterCallback, since it would waste more memory.
// Ideally we'd have a C-style callback and a user-data pointer, instead of being forced to create a class.
class ControllerFilter : public PxQueryFilterCallback
{
public:
PxQueryHitType::Enum preFilter(const PxFilterData& filterData, const PxShape* shape, const PxRigidActor* actor, PxHitFlags& queryFlags)
{
// PT: ignore triggers
if(shape->getFlags() & physx::PxShapeFlag::eTRIGGER_SHAPE)
return PxQueryHitType::eNONE;
// PT: we want to discard our own internal shapes only
if(mShapeHashSet->contains(const_cast<PxShape*>(shape)))
return PxQueryHitType::eNONE;
// PT: otherwise we revert to the user-callback, if it exists, and if users enabled that call
if(mUserFilterCallback && (mUserFilterFlags | PxQueryFlag::ePREFILTER))
return mUserFilterCallback->preFilter(filterData, shape, actor, queryFlags);
return PxQueryHitType::eBLOCK;
}
PxQueryHitType::Enum postFilter(const PxFilterData& filterData, const PxQueryHit& hit)
{
// PT: we may get called if users have asked for such a callback
if(mUserFilterCallback && (mUserFilterFlags | PxQueryFlag::ePOSTFILTER))
return mUserFilterCallback->postFilter(filterData, hit);
PX_ASSERT(0); // PT: otherwise we shouldn't have been called
return PxQueryHitType::eNONE;
}
Ps::HashSet<PxShape*>* mShapeHashSet;
PxQueryFilterCallback* mUserFilterCallback;
PxQueryFlags mUserFilterFlags;
};
bool Controller::filterTouchedShape(const PxControllerFilters& filters)
{
PxQueryFlags filterFlags = PxQueryFlag::eDYNAMIC|PxQueryFlag::ePREFILTER;
PxQueryFilterData filterData(filters.mFilterData ? *filters.mFilterData : PxFilterData(), filterFlags);
PxHitFlags hitFlags = PxHitFlags(0);
if(filters.mFilterCallback && (filters.mFilterFlags | PxQueryFlag::ePREFILTER))
{
PxQueryHitType::Enum retVal = filters.mFilterCallback->preFilter(filterData.data, mCctModule.mTouchedShape.get(), mCctModule.mTouchedActor.get(), hitFlags);
if(retVal != PxQueryHitType::eNONE)
return true;
else
return false;
}
return true;
}
void Controller::findTouchedObject(const PxControllerFilters& filters, const PxObstacleContext* obstacleContext, const PxVec3& upDirection)
{
PX_ASSERT(!mCctModule.mTouchedShape && (mCctModule.mTouchedObstacleHandle == INVALID_OBSTACLE_HANDLE));
// PT: the CCT works perfectly on statics without this extra mechanism, so we only raycasts against dynamics.
// The pre-filter callback is used to filter out our own proxy actor shapes. We need to make sure our own filter
// doesn't disturb the user-provided filter(s).
// PT: for starter, if user doesn't want to collide against dynamics, we can skip the whole thing
if(filters.mFilterFlags & PxQueryFlag::eDYNAMIC)
{
ControllerFilter preFilter;
preFilter.mShapeHashSet = &mManager->mCCTShapes;
preFilter.mUserFilterCallback = filters.mFilterCallback;
preFilter.mUserFilterFlags = filters.mFilterFlags;
// PT: for our own purpose we just want dynamics & pre-filter
PxQueryFlags filterFlags = PxQueryFlag::eDYNAMIC|PxQueryFlag::ePREFILTER;
// PT: but we may need the post-filter callback as well if users want it
if(filters.mFilterFlags & PxQueryFlag::ePOSTFILTER)
filterFlags |= PxQueryFlag::ePOSTFILTER;
PxQueryFilterData filterData(filters.mFilterData ? *filters.mFilterData : PxFilterData(), filterFlags);
const PxF32 probeLength = getHalfHeightInternal(); // Distance to feet
const PxF32 extra = 0.0f;//probeLength * 0.1f;
const PxVec3 rayOrigin = toVec3(mPosition);
PxRaycastBuffer hit;
hit.block.distance = FLT_MAX;
if(mScene->raycast(rayOrigin, -upDirection, probeLength+extra, hit, PxHitFlags(0), filterData, &preFilter))
{
// copy touching hit to blocking so that the rest of the code works with .block
hit.block = hit.getAnyHit(0);
PX_ASSERT(hit.block.shape);
PX_ASSERT(hit.block.actor);
PX_ASSERT(hit.block.distance<=probeLength+extra);
mCctModule.mTouchedShape = hit.block.shape;
mCctModule.mTouchedActor = hit.block.actor;
// mCctModule.mTouchedPos = getShapeGlobalPose(*hit.shape).p - upDirection*(probeLength-hit.distance);
// PT: we only care about the up delta here
const PxTransform shapeTransform = getShapeGlobalPose(*hit.block.shape, *hit.block.actor);
mCctModule.mTouchedPosShape_World = PxVec3(0) - upDirection*(probeLength-hit.block.distance);
mCctModule.mTouchedPosShape_Local = shapeTransform.transformInv(PxVec3(0));
mPreviousSceneTimestamp = mScene->getTimestamp()-1; // PT: just make sure cached timestamp is different
}
if(obstacleContext)
{
const ObstacleContext* obstacles = static_cast<const ObstacleContext*>(obstacleContext);
PxRaycastHit obstacleHit;
ObstacleHandle obstacleHandle;
const PxObstacle* touchedObstacle = obstacles->raycastSingle(obstacleHit, rayOrigin, -upDirection, probeLength+extra, obstacleHandle);
// printf("Touched raycast obstacle: %d\n", int(touchedObstacle));
if(touchedObstacle && obstacleHit.distance<hit.block.distance)
{
PX_ASSERT(obstacleHit.distance<=probeLength+extra);
mCctModule.mTouchedObstacleHandle = obstacleHandle;
if(!gUseLocalSpace)
{
mCctModule.mTouchedPos = toVec3(touchedObstacle->mPos) - upDirection*(probeLength-obstacleHit.distance);
}
else
{
// PT: we only care about the up delta here
mCctModule.mTouchedPosObstacle_World = PxVec3(0) - upDirection*(probeLength-obstacleHit.distance);
mCctModule.mTouchedPosObstacle_Local = worldToLocal(*touchedObstacle, PxExtendedVec3(0,0,0));
}
}
}
}
}
bool Controller::rideOnTouchedObject(SweptVolume& volume, const PxVec3& upDirection, PxVec3& disp, const PxObstacleContext* obstacleContext)
{
PX_ASSERT(mCctModule.mTouchedShape || (mCctModule.mTouchedObstacleHandle != INVALID_OBSTACLE_HANDLE));
bool standingOnMoving = false;
bool canDoUpdate = true; // Always true on obstacles
PxU32 behaviorFlags = 0; // Default on shapes
PxVec3 delta(0);
float timeCoeff = 1.0f;
if(mCctModule.mTouchedShape)
{
// PT: riding on a shape
// PT: it is important to skip this stuff for static meshes,
// otherwise accuracy issues create bugs like TA14007.
const PxRigidActor& rigidActor = *mCctModule.mTouchedActor.get();
if(rigidActor.getConcreteType()!=PxConcreteType::eRIGID_STATIC)
{
// PT: we only do the update when the timestamp has changed, otherwise "delta" will be zero
// even if the underlying shape is moving.
const PxU32 timestamp = mScene->getTimestamp();
// printf("TimeStamp: %d\n", timestamp);
canDoUpdate = timestamp!=mPreviousSceneTimestamp;
if(canDoUpdate)
{
mPreviousSceneTimestamp = timestamp;
timeCoeff = computeTimeCoeff();
if(mBehaviorCallback)
behaviorFlags = mBehaviorCallback->getBehaviorFlags(*mCctModule.mTouchedShape.get(), *mCctModule.mTouchedActor.get());
// delta = getShapeGlobalPose(*mCctModule.mTouchedShape).p - mCctModule.mTouchedPos;
const PxTransform shapeTransform = getShapeGlobalPose(*mCctModule.mTouchedShape.get(), rigidActor);
const PxVec3 posPreviousFrame = mCctModule.mTouchedPosShape_World;
const PxVec3 posCurrentFrame = shapeTransform.transform(mCctModule.mTouchedPosShape_Local);
delta = posCurrentFrame - posPreviousFrame;
}
}
}
else
{
// PT: riding on an obstacle
behaviorFlags = PxControllerBehaviorFlag::eCCT_CAN_RIDE_ON_OBJECT; // Default on obstacles
timeCoeff = computeTimeCoeff();
const PxObstacle* touchedObstacle = obstacleContext->getObstacleByHandle(mCctModule.mTouchedObstacleHandle);
PX_ASSERT(touchedObstacle);
if(mBehaviorCallback)
behaviorFlags = mBehaviorCallback->getBehaviorFlags(*touchedObstacle);
if(!gUseLocalSpace)
{
delta = toVec3(touchedObstacle->mPos) - mCctModule.mTouchedPos;
}
else
{
PxVec3 posPreviousFrame = mCctModule.mTouchedPosObstacle_World;
PxVec3 posCurrentFrame = localToWorld(*touchedObstacle, mCctModule.mTouchedPosObstacle_Local);
delta = posCurrentFrame - posPreviousFrame;
}
}
if(canDoUpdate && !(behaviorFlags & PxControllerBehaviorFlag::eCCT_USER_DEFINED_RIDE))
{
// PT: amazingly enough even isAlmostZero doesn't solve this one.
// Moving on a static mesh sometimes produces delta bigger than 1e-6f!
// This may also explain the drift on some rotating platforms. It looks
// like this delta computation is not very accurate.
// standingOnMoving = !delta.isZero();
standingOnMoving = !Ps::isAlmostZero(delta);
mCachedStandingOnMoving = standingOnMoving;
//printf("%f %f %f\n", delta.x, delta.y, delta.z);
if(standingOnMoving)
{
const float dir_dot_up = delta.dot(upDirection);
const bool deltaMovingUp = dir_dot_up>0.0f;
PxVec3 deltaUpDisp, deltaSideDisp;
Ps::decomposeVector(deltaUpDisp, deltaSideDisp, delta, upDirection);
if(deltaMovingUp)
{
volume.mCenter.x += PxExtended(deltaUpDisp.x);
volume.mCenter.y += PxExtended(deltaUpDisp.y);
volume.mCenter.z += PxExtended(deltaUpDisp.z);
}
else
{
disp += deltaUpDisp;
}
if(behaviorFlags & PxControllerBehaviorFlag::eCCT_CAN_RIDE_ON_OBJECT)
disp += deltaSideDisp;
}
// printf("delta in: %f %f %f (%f)\n", delta.x, delta.y, delta.z, 1.0f/timeCoeff);
mDeltaXP = delta * timeCoeff;
}
else
{
standingOnMoving = mCachedStandingOnMoving;
}
// mDelta = delta;
return standingOnMoving;
}
PxControllerCollisionFlags Controller::move(SweptVolume& volume, const PxVec3& originalDisp, PxF32 minDist, PxF32 elapsedTime, const PxControllerFilters& filters, const PxObstacleContext* obstacleContext, bool constrainedClimbingMode)
{
const bool lockWrite = mManager->mLockingEnabled;
if(lockWrite)
mWriteLock.lock();
mGlobalTime += PxF64(elapsedTime);
// Init CCT with per-controller settings
RenderBuffer* renderBuffer = mManager->mRenderBuffer;
const PxU32 debugRenderFlags = mManager->mDebugRenderingFlags;
mCctModule.mRenderBuffer = renderBuffer;
mCctModule.mRenderFlags = debugRenderFlags;
mCctModule.mUserParams = mUserParams;
mCctModule.mFlags |= STF_FIRST_UPDATE;
mCctModule.mUserParams.mMaxEdgeLength2 = mManager->mMaxEdgeLength * mManager->mMaxEdgeLength;
mCctModule.mUserParams.mTessellation = mManager->mTessellation;
mCctModule.mUserParams.mOverlapRecovery = mManager->mOverlapRecovery;
mCctModule.mUserParams.mPreciseSweeps = mManager->mPreciseSweeps;
mCctModule.mUserParams.mPreventVerticalSlidingAgainstCeiling = mManager->mPreventVerticalSlidingAgainstCeiling;
mCctModule.resetStats();
const PxVec3& upDirection = mUserParams.mUpDirection;
///////////
PxVec3 disp = originalDisp + mOverlapRecover;
mOverlapRecover = PxVec3(0.0f);
bool standingOnMoving = false; // PT: whether the CCT is currently standing on a moving object
//printf("Touched shape: %d\n", int(mCctModule.mTouchedShape));
//standingOnMoving=true;
// printf("Touched obstacle: %d\n", int(mCctModule.mTouchedObstacle));
if(mCctModule.mTouchedActor && mCctModule.mTouchedShape)
{
PxU32 nbShapes = mCctModule.mTouchedActor->getNbShapes();
bool found = false;
for(PxU32 i=0;i<nbShapes;i++)
{
PxShape* shape = NULL;
mCctModule.mTouchedActor->getShapes(&shape, 1, i);
if(mCctModule.mTouchedShape==shape)
{
found = true;
break;
}
}
if(!found)
{
mCctModule.mTouchedActor = NULL;
mCctModule.mTouchedShape = NULL;
}
else
{
// check if we are still in the same scene
if(mCctModule.mTouchedActor->getScene() != mScene)
{
mCctModule.mTouchedShape = NULL;
mCctModule.mTouchedActor = NULL;
}
else
{
// check if the shape still does have the sq flag
if(!(mCctModule.mTouchedShape->getFlags() & PxShapeFlag::eSCENE_QUERY_SHAPE))
{
mCctModule.mTouchedShape = NULL;
mCctModule.mTouchedActor = NULL;
}
else
{
// invoke the CCT filtering for the shape
if(!filterTouchedShape(filters))
{
mCctModule.mTouchedShape = NULL;
mCctModule.mTouchedActor = NULL;
}
}
}
}
}
if(!mCctModule.mTouchedShape && (mCctModule.mTouchedObstacleHandle == INVALID_OBSTACLE_HANDLE))
findTouchedObject(filters, obstacleContext, upDirection);
if(mCctModule.mTouchedShape || (mCctModule.mTouchedObstacleHandle != INVALID_OBSTACLE_HANDLE))
{
standingOnMoving = rideOnTouchedObject(volume, upDirection, disp, obstacleContext);
}
else
{
mCachedStandingOnMoving = false;
mDeltaXP = PxVec3(0.0f);
}
// printf("standingOnMoving: %d\n", standingOnMoving);
///////////
Ps::Array<const void*>& boxUserData = mManager->mBoxUserData;
Ps::Array<PxExtendedBox>& boxes = mManager->mBoxes;
Ps::Array<const void*>& capsuleUserData = mManager->mCapsuleUserData;
Ps::Array<PxExtendedCapsule>& capsules = mManager->mCapsules;
PX_ASSERT(!boxUserData.size());
PX_ASSERT(!boxes.size());
PX_ASSERT(!capsuleUserData.size());
PX_ASSERT(!capsules.size());
{
PX_PROFILE_ZONE("CharacterController.filterCandidateControllers", getContextId());
// Experiment - to do better
const PxU32 nbControllers = mManager->getNbControllers();
Controller** controllers = mManager->getControllers();
for(PxU32 i=0;i<nbControllers;i++)
{
Controller* currentController = controllers[i];
if(currentController==this)
continue;
bool keepController = true;
if(filters.mCCTFilterCallback)
keepController = filters.mCCTFilterCallback->filter(*getPxController(), *currentController->getPxController());
if(keepController)
{
if(currentController->mType==PxControllerShapeType::eBOX)
{
// PT: TODO: optimize this
BoxController* BC = static_cast<BoxController*>(currentController);
PxExtendedBox obb;
BC->getOBB(obb);
boxes.pushBack(obb);
#ifdef REMOVED
if(renderBuffer /*&& (debugRenderFlags & PxControllerDebugRenderFlag::eOBSTACLES)*/)
{
RenderOutput out(*renderBuffer);
out << gCCTBoxDebugColor;
out << PxTransform(toVec3(obb.center), obb.rot);
out << DebugBox(obb.extents, true);
}
#endif
const size_t code = encodeUserObject(i, USER_OBJECT_CCT);
boxUserData.pushBack(reinterpret_cast<const void*>(code));
}
else if(currentController->mType==PxControllerShapeType::eCAPSULE)
{
CapsuleController* CC = static_cast<CapsuleController*>(currentController);
// PT: TODO: optimize this
PxExtendedCapsule worldCapule;
CC->getCapsule(worldCapule);
capsules.pushBack(worldCapule);
const size_t code = encodeUserObject(i, USER_OBJECT_CCT);
capsuleUserData.pushBack(reinterpret_cast<const void*>(code));
}
else PX_ASSERT(0);
}
}
}
const ObstacleContext* obstacles = NULL;
if(obstacleContext)
{
obstacles = static_cast<const ObstacleContext*>(obstacleContext);
// PT: TODO: optimize this
const PxU32 nbExtraBoxes = obstacles->mBoxObstacles.size();
for(PxU32 i=0;i<nbExtraBoxes;i++)
{
const PxBoxObstacle& userBoxObstacle = obstacles->mBoxObstacles[i].mData;
PxExtendedBox extraBox;
extraBox.center = userBoxObstacle.mPos;
extraBox.extents = userBoxObstacle.mHalfExtents;
extraBox.rot = userBoxObstacle.mRot;
boxes.pushBack(extraBox);
const size_t code = encodeUserObject(i, USER_OBJECT_BOX_OBSTACLE);
boxUserData.pushBack(reinterpret_cast<const void*>(code));
if(renderBuffer && (debugRenderFlags & PxControllerDebugRenderFlag::eOBSTACLES))
{
RenderOutput out(*renderBuffer);
out << gObstacleDebugColor;
out << PxTransform(toVec3(userBoxObstacle.mPos), userBoxObstacle.mRot);
out << DebugBox(userBoxObstacle.mHalfExtents, true);
}
}
const PxU32 nbExtraCapsules = obstacles->mCapsuleObstacles.size();
for(PxU32 i=0;i<nbExtraCapsules;i++)
{
const PxCapsuleObstacle& userCapsuleObstacle = obstacles->mCapsuleObstacles[i].mData;
PxExtendedCapsule extraCapsule;
const PxVec3 capsuleAxis = userCapsuleObstacle.mRot.getBasisVector0() * userCapsuleObstacle.mHalfHeight;
extraCapsule.p0 = PxExtendedVec3( userCapsuleObstacle.mPos.x - PxExtended(capsuleAxis.x),
userCapsuleObstacle.mPos.y - PxExtended(capsuleAxis.y),
userCapsuleObstacle.mPos.z - PxExtended(capsuleAxis.z));
extraCapsule.p1 = PxExtendedVec3( userCapsuleObstacle.mPos.x + PxExtended(capsuleAxis.x),
userCapsuleObstacle.mPos.y + PxExtended(capsuleAxis.y),
userCapsuleObstacle.mPos.z + PxExtended(capsuleAxis.z));
extraCapsule.radius = userCapsuleObstacle.mRadius;
capsules.pushBack(extraCapsule);
const size_t code = encodeUserObject(i, USER_OBJECT_CAPSULE_OBSTACLE);
capsuleUserData.pushBack(reinterpret_cast<const void*>(code));
if(renderBuffer && (debugRenderFlags & PxControllerDebugRenderFlag::eOBSTACLES))
{
RenderOutput out(*renderBuffer);
out << gObstacleDebugColor;
out.outputCapsule(userCapsuleObstacle.mRadius, userCapsuleObstacle.mHalfHeight, PxTransform(toVec3(userCapsuleObstacle.mPos), userCapsuleObstacle.mRot));
}
}
}
UserObstacles userObstacles;
const PxU32 nbBoxes = boxes.size();
userObstacles.mNbBoxes = nbBoxes;
userObstacles.mBoxes = nbBoxes ? boxes.begin() : NULL;
userObstacles.mBoxUserData = nbBoxes ? boxUserData.begin() : NULL;
const PxU32 nbCapsules = capsules.size();
userObstacles.mNbCapsules = nbCapsules;
userObstacles.mCapsules = nbCapsules ? capsules.begin() : NULL;
userObstacles.mCapsuleUserData = nbCapsules ? capsuleUserData.begin() : NULL;
PxInternalCBData_OnHit userHitData;
userHitData.controller = this;
userHitData.obstacles = obstacles;
///////////
PxControllerCollisionFlags collisionFlags = PxControllerCollisionFlags(0);
PxInternalCBData_FindTouchedGeom findGeomData;
findGeomData.scene = mScene;
findGeomData.renderBuffer = renderBuffer;
findGeomData.cctShapeHashSet = &mManager->mCCTShapes;
mCctModule.mFlags &= ~STF_WALK_EXPERIMENT;
// store new touched actor/shape. Then set new actor/shape to avoid register/unregister for same objects
const PxRigidActor* touchedActor = NULL;
const PxShape* touchedShape = NULL;
PxExtendedVec3 Backup = volume.mCenter;
collisionFlags = mCctModule.moveCharacter(&findGeomData, &userHitData, volume, disp, userObstacles, minDist, filters, constrainedClimbingMode, standingOnMoving, touchedActor, touchedShape, getContextId());
if(mCctModule.mFlags & STF_HIT_NON_WALKABLE)
{
// A bit slow, but everything else I tried was less convincing...
mCctModule.mFlags |= STF_WALK_EXPERIMENT;
volume.mCenter = Backup;
PxVec3 xpDisp;
if(mUserParams.mNonWalkableMode==PxControllerNonWalkableMode::ePREVENT_CLIMBING_AND_FORCE_SLIDING)
{
PxVec3 tangent_compo;
Ps::decomposeVector(xpDisp, tangent_compo, disp, upDirection);
}
else xpDisp = disp;
collisionFlags = mCctModule.moveCharacter(&findGeomData, &userHitData, volume, xpDisp, userObstacles, minDist, filters, constrainedClimbingMode, standingOnMoving, touchedActor, touchedShape, getContextId());
mCctModule.mFlags &= ~STF_WALK_EXPERIMENT;
}
mCctModule.mTouchedActor = touchedActor;
mCctModule.mTouchedShape = touchedShape;
mCollisionFlags = collisionFlags;
// Copy results back
mPosition = volume.mCenter;
// Update kinematic actor
if(mKineActor)
{
const PxVec3 delta = Backup - volume.mCenter;
const PxF32 deltaM2 = delta.magnitudeSquared();
if(deltaM2!=0.0f)
{
PxTransform targetPose = mKineActor->getGlobalPose();
targetPose.p = toVec3(mPosition);
targetPose.q = mUserParams.mQuatFromUp;
mKineActor->setKinematicTarget(targetPose);
}
}
mManager->resetObstaclesBuffers();
if (lockWrite)
mWriteLock.unlock();
return collisionFlags;
}
PxControllerCollisionFlags BoxController::move(const PxVec3& disp, PxF32 minDist, PxF32 elapsedTime, const PxControllerFilters& filters, const PxObstacleContext* obstacles)
{
PX_PROFILE_ZONE("CharacterController.move", getContextId());
PX_SIMD_GUARD;
// Create internal swept box
SweptBox sweptBox;
sweptBox.mCenter = mPosition;
sweptBox.mExtents = PxVec3(mHalfHeight, mHalfSideExtent, mHalfForwardExtent);
sweptBox.mHalfHeight = mHalfHeight; // UBI
return Controller::move(sweptBox, disp, minDist, elapsedTime, filters, obstacles, false);
}
PxControllerCollisionFlags CapsuleController::move(const PxVec3& disp, PxF32 minDist, PxF32 elapsedTime, const PxControllerFilters& filters, const PxObstacleContext* obstacles)
{
PX_PROFILE_ZONE("CharacterController.move", getContextId());
PX_SIMD_GUARD;
// Create internal swept capsule
SweptCapsule sweptCapsule;
sweptCapsule.mCenter = mPosition;
sweptCapsule.mRadius = mRadius;
sweptCapsule.mHeight = mHeight;
sweptCapsule.mHalfHeight = mHeight*0.5f + mRadius; // UBI
return Controller::move(sweptCapsule, disp, minDist, elapsedTime, filters, obstacles, mClimbingMode==PxCapsuleClimbingMode::eCONSTRAINED);
}
Reference in New Issue View Git Blame Copy Permalink