// // 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-2018 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 "Ps.h" #include "GuVecCapsule.h" #include "GuVecBox.h" #include "GuVecConvexHull.h" #include "GuVecTriangle.h" #include "GuVecShrunkConvexHull.h" #include "GuVecShrunkBox.h" #include "GuGJKRaycast.h" #include "GuCCDSweepConvexMesh.h" #include "GuGJKType.h" namespace physx { namespace Gu { using namespace Ps::aos; template PX_FORCE_INLINE PxReal getRadius(const PxGeometry&) { return 0; } template<> PX_FORCE_INLINE PxReal getRadius(const PxGeometry& g) { PX_ASSERT(g.getType() == PxGeometryType::eCAPSULE || g.getType() == PxGeometryType::eSPHERE); PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(PxSphereGeometry, radius) == PX_OFFSET_OF(PxCapsuleGeometry, radius)); return static_cast(g).radius; } template static PxReal CCDSweep(ConvexA& a, ConvexB& b, const PxTransform& transform0, const PxTransform& transform1, const PxTransform& lastTm0, const PxTransform& lastTm1, const Ps::aos::FloatV& toiEstimate, PxVec3& worldPoint, PxVec3& worldNormal, PxReal inflation = 0.f) { PX_UNUSED(toiEstimate); //KS - TODO - can we use this again? using namespace Ps::aos; const Vec3V zero = V3Zero(); const QuatV q0 = QuatVLoadA(&transform0.q.x); const Vec3V p0 = V3LoadA(&lastTm0.p.x); const QuatV q1 = QuatVLoadA(&transform1.q.x); const Vec3V p1 = V3LoadA(&lastTm1.p.x); const PsTransformV tr0(p0, q0); const PsTransformV tr1(p1, q1); const PsMatTransformV aToB(tr1.transformInv(tr0)); const Vec3V trans0p = V3LoadU(transform0.p); const Vec3V trans1p = V3LoadU(transform1.p); const Vec3V trA = V3Sub(trans0p, p0); const Vec3V trB = V3Sub(trans1p, p1); const Vec3V relTr = tr1.rotateInv(V3Sub(trB, trA)); FloatV lambda; Vec3V closestA, normal; const FloatV initialLambda = FZero(); const RelativeConvex convexA(a, aToB); const LocalConvex convexB(b); if(gjkRaycastPenetration, LocalConvex >(convexA, convexB, aToB.p, initialLambda, zero, relTr, lambda, normal, closestA, inflation, true)) { //Adjust closestA because it will be on the surface of convex a in its initial position (s). If the TOI > 0, we need to move //the point along the sweep direction to get the world-space hit position. PxF32 res; FStore(lambda, &res); closestA = V3ScaleAdd(trA, FMax(lambda, FZero()), tr1.transform(closestA)); normal = tr1.rotate(normal); V3StoreU(normal, worldNormal); V3StoreU(closestA, worldPoint); return res; } return PX_MAX_REAL; } // // lookup table for geometry-vs-geometry sweeps // PxReal UnimplementedSweep (GU_SWEEP_METHOD_ARGS_UNUSED) { return PX_MAX_REAL; //no impact } template PxReal SweepGeomGeom(GU_SWEEP_METHOD_ARGS) { PX_UNUSED(outCCDFaceIndex); PX_UNUSED(fastMovingThreshold); const PxGeometry& g0 = shape0.mGeometry->getGeometry(); const PxGeometry& g1 = shape1.mGeometry->getGeometry(); //Geom0 geom0(g0); //Geom1 geom1(g1); typename Shrink::Type geom0(g0); typename Shrink::Type geom1(g1); return CCDSweep(geom0, geom1, transform0, transform1, lastTm0, lastTm1, FLoad(toiEstimate), worldPoint, worldNormal, restDistance+getRadius(g0)+getRadius(g1) ); } typedef PxReal (*SweepMethod) (GU_SWEEP_METHOD_ARGS); PxReal SweepAnyShapeHeightfield(GU_SWEEP_METHOD_ARGS); PxReal SweepAnyShapeMesh(GU_SWEEP_METHOD_ARGS); SweepMethod g_SweepMethodTable[PxGeometryType::eGEOMETRY_COUNT][PxGeometryType::eGEOMETRY_COUNT] = { //PxGeometryType::eSPHERE { SweepGeomGeom, //PxGeometryType::eSPHERE UnimplementedSweep, //PxGeometryType::ePLANE SweepGeomGeom, //PxGeometryType::eCAPSULE SweepGeomGeom, //PxGeometryType::eBOX SweepGeomGeom, //PxGeometryType::eCONVEXMESH SweepAnyShapeMesh, //PxGeometryType::eTRIANGLEMESH SweepAnyShapeHeightfield, //PxGeometryType::eHEIGHTFIELD //TODO }, //PxGeometryType::ePLANE { 0, //PxGeometryType::eSPHERE UnimplementedSweep, //PxGeometryType::ePLANE UnimplementedSweep, //PxGeometryType::eCAPSULE UnimplementedSweep, //PxGeometryType::eBOX UnimplementedSweep, //PxGeometryType::eCONVEXMESH UnimplementedSweep, //PxGeometryType::eTRIANGLEMESH UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD }, //PxGeometryType::eCAPSULE { 0, //PxGeometryType::eSPHERE 0, //PxGeometryType::ePLANE SweepGeomGeom, //PxGeometryType::eCAPSULE SweepGeomGeom, //PxGeometryType::eBOX SweepGeomGeom, //PxGeometryType::eCONVEXMESH SweepAnyShapeMesh, //PxGeometryType::eTRIANGLEMESH SweepAnyShapeHeightfield, //PxGeometryType::eHEIGHTFIELD }, //PxGeometryType::eBOX { 0, //PxGeometryType::eSPHERE 0, //PxGeometryType::ePLANE 0, //PxGeometryType::eCAPSULE SweepGeomGeom, //PxGeometryType::eBOX SweepGeomGeom, //PxGeometryType::eCONVEXMESH SweepAnyShapeMesh, //PxGeometryType::eTRIANGLEMESH SweepAnyShapeHeightfield, //PxGeometryType::eHEIGHTFIELD }, //PxGeometryType::eCONVEXMESH { 0, //PxGeometryType::eSPHERE 0, //PxGeometryType::ePLANE 0, //PxGeometryType::eCAPSULE 0, //PxGeometryType::eBOX SweepGeomGeom, //PxGeometryType::eCONVEXMESH SweepAnyShapeMesh, //PxGeometryType::eTRIANGLEMESH SweepAnyShapeHeightfield, //PxGeometryType::eHEIGHTFIELD }, //PxGeometryType::eTRIANGLEMESH { 0, //PxGeometryType::eSPHERE 0, //PxGeometryType::ePLANE 0, //PxGeometryType::eCAPSULE 0, //PxGeometryType::eBOX 0, //PxGeometryType::eCONVEXMESH UnimplementedSweep, //PxGeometryType::eTRIANGLEMESH UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD }, //PxGeometryType::eHEIGHTFIELD { 0, //PxGeometryType::eSPHERE 0, //PxGeometryType::ePLANE 0, //PxGeometryType::eCAPSULE 0, //PxGeometryType::eBOX 0, //PxGeometryType::eCONVEXMESH 0, //PxGeometryType::eTRIANGLEMESH UnimplementedSweep, //PxGeometryType::eHEIGHTFIELD }, }; PxReal SweepShapeShape(GU_SWEEP_METHOD_ARGS) { PxGeometryType::Enum type0 = shape0.mGeometry->getType(); PxGeometryType::Enum type1 = shape1.mGeometry->getType(); return g_SweepMethodTable[type0][type1](shape0, shape1, transform0, transform1, lastTm0, lastTm1, restDistance, worldNormal, worldPoint, toiEstimate, outCCDFaceIndex, fastMovingThreshold); } // // lookup table for sweeps agains triangles // PxReal UnimplementedTriangleSweep(GU_TRIANGLE_SWEEP_METHOD_ARGS) { PX_UNUSED(shape0); PX_UNUSED(shape1); PX_UNUSED(transform0); PX_UNUSED(transform1); PX_UNUSED(lastTm0); PX_UNUSED(lastTm1); PX_UNUSED(restDistance); PX_UNUSED(worldNormal); PX_UNUSED(worldPoint); PX_UNUSED(meshScaling); PX_UNUSED(triangle); PX_UNUSED(toiEstimate); return 1e10f; //no impact } template PxReal SweepGeomTriangles(GU_TRIANGLE_SWEEP_METHOD_ARGS) { PX_UNUSED(meshScaling); PX_UNUSED(shape1); const PxGeometry& g = shape0.getGeometry(); //Geom geom(g); typename Shrink::Type geom(g); return CCDSweep(triangle, geom, transform1, transform0, lastTm1, lastTm0, FLoad(toiEstimate), worldPoint, worldNormal, restDistance+getRadius(g) ); } typedef PxReal (*TriangleSweepMethod) (GU_TRIANGLE_SWEEP_METHOD_ARGS); TriangleSweepMethod g_TriangleSweepMethodTable[PxGeometryType::eGEOMETRY_COUNT] = { SweepGeomTriangles, //PxGeometryType::eSPHERE UnimplementedTriangleSweep, //PxGeometryType::ePLANE SweepGeomTriangles, //PxGeometryType::eCAPSULE SweepGeomTriangles, //PxGeometryType::eBOX SweepGeomTriangles, //PxGeometryType::eCONVEXMESH UnimplementedTriangleSweep, //PxGeometryType::eTRIANGLEMESH UnimplementedTriangleSweep, //PxGeometryType::eHEIGHTFIELD }; PxReal SweepShapeTriangle(GU_TRIANGLE_SWEEP_METHOD_ARGS) { const PxGeometryType::Enum type0 = shape0.getType(); TriangleSweepMethod method = g_TriangleSweepMethodTable[type0]; return method(shape0, shape1, transform0, transform1, lastTm0, lastTm1, restDistance, worldNormal, worldPoint, meshScaling, triangle, toiEstimate); } } }