// // 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 "GuContactMethodImpl.h" #include "GuGeometryUnion.h" #include "GuHeightFieldUtil.h" #include "CmRenderBuffer.h" #include "GuContactBuffer.h" #include "GuContactMethodImpl.h" #include "GuLegacyTraceLineCallback.h" #include "PsFPU.h" #include "CmMatrix34.h" using namespace physx; #define DISTANCE_BASED_TEST ///////// #if 0 #include "CmRenderOutput.h" #include "PxsContext.h" static void gVisualizeBox(const Gu::Box& box, PxcNpThreadContext& context, PxU32 color=0xffffff) { PxMat33 rot(box.rot.column0, box.rot.column1, box.rot.column2); PxMat44 m(rot, box.center); Cm::DebugBox db(box.extents); Cm::RenderOutput& out = context.mRenderOutput; out << color << m; out << db; } static void gVisualizeLine(const PxVec3& a, const PxVec3& b, PxcNpThreadContext& context, PxU32 color=0xffffff) { PxMat44 m = PxMat44(PxIdentity); Cm::RenderOutput& out = context.mRenderOutput; out << color << m << Cm::RenderOutput::LINES << a << b; } #endif ///////// // ptchernev TODO: make sure these are ok before shipping static const bool gCompileBoxVertex = true; static const bool gCompileEdgeEdge = true; static const bool gCompileHeightFieldVertex = true; static const PxReal signs[24] = { -1,-1,-1, -1,-1, 1, -1, 1,-1, -1, 1, 1, 1,-1,-1, 1,-1, 1, 1, 1,-1, 1, 1, 1, }; static const PxU8 edges[24] = { 0,1, 1,3, 3,2, 2,0, 4,5, 5,7, 7,6, 6,4, 0,4, 1,5, 2,6, 3,7, }; static bool GuDepenetrateBox( const PxVec3& point, const PxVec3& safeNormal, const PxVec3& dimensions, float /*contactDistance*/, PxVec3& normal, PxReal& distance) { PxVec3 faceNormal(PxReal(0)); PxReal distance1 = -PX_MAX_REAL; // cant be more PxReal distance2 = -PX_MAX_REAL; // cant be more PxI32 poly1 = -1; PxI32 poly2 = -2; for (PxU32 poly = 0; poly < 6; poly++) { PxU32 dim = poly % 3; PxReal sign = (poly > 2) ? -PxReal(1) : PxReal(1); PxVec3 n(PxVec3(0)); n[dim] = sign; PxReal proj = n[dim] * safeNormal[dim]; PxReal d = n[dim] * (point[dim] - sign * dimensions[dim]); #ifdef DISTANCE_BASED_TEST // PT: I'm not really sure about contactDistance here // AP: enabling this causes jitter in DE2740 //d -= contactDistance; #endif if (d >= 0) return false; if (proj > 0) { if (d > distance1) // less penetration { distance1 = d; faceNormal = n; poly1 = PxI32(poly); } // distance2 / d = 1 / proj PxReal tmp = d / proj; if (tmp > distance2) { distance2 = tmp; poly2 = PxI32(poly); } } } if (poly1 == poly2) { PX_ASSERT(faceNormal.magnitudeSquared() != 0.0f); normal = faceNormal; distance = -distance1; } else { normal = safeNormal; distance = -distance2; } return true; } //Box-Heightfield and Convex-Heightfield do not support positive values for contactDistance, //and if in this case we would emit contacts normally, we'd cause things to jitter. //as a workaround we add contactDistance to the distance values that we emit in contacts. //this has the effect that the biasing will work exactly as if we had specified a legacy skinWidth of (contactDistance - restDistance) namespace physx { namespace Gu { bool legacyContactBoxHeightfield(GU_CONTACT_METHOD_ARGS) { PX_UNUSED(renderOutput); PX_UNUSED(cache); //PXC_WARN_ONCE(contactDistance > 0.0f, "PxcContactBoxHeightField: Box-Heightfield does not support distance based contact generation! Ignoring contactOffset > 0!"); // Get actual shape data const PxBoxGeometry& shapeBox = shape0.get(); const PxHeightFieldGeometryLL& hfGeom = shape1.get(); const Gu::HeightFieldTraceUtil hfUtil(hfGeom); const Gu::HeightField& hf = hfUtil.getHeightField(); PX_ASSERT(contactBuffer.count==0); Cm::Matrix34 boxShapeAbsPose(transform0); Cm::Matrix34 hfShapeAbsPose(transform1); const PxMat33& left = hfShapeAbsPose.m; const PxMat33& right = boxShapeAbsPose.m; Cm::Matrix34 boxShape2HfShape(left.getInverse()* right, left.getInverse()*(boxShapeAbsPose.p - hfShapeAbsPose.p)); // Get box vertices. PxVec3 boxVertices[8]; PxVec3 boxVertexNormals[8]; for(PxU32 i=0; i<8; i++) { boxVertices[i] = PxVec3(shapeBox.halfExtents.x*signs[3*i], shapeBox.halfExtents.y*signs[3*i+1], shapeBox.halfExtents.z*signs[3*i+2]); boxVertexNormals[i] = PxVec3(signs[3*i], signs[3*i+1], signs[3*i+2]); boxVertexNormals[i].normalize(); } // Transform box vertices to HeightFieldShape space. PxVec3 boxVerticesInHfShape[8]; PxVec3 boxVertexNormalsInHfShape[8]; for(PxU32 i=0; i<8; i++) { boxVerticesInHfShape[i] = boxShape2HfShape.transform(boxVertices[i]); boxVertexNormalsInHfShape[i] = boxShape2HfShape.rotate(boxVertexNormals[i]); } // bounds of box on HeightField. PxVec3 aabbMin(boxVerticesInHfShape[0]); PxVec3 aabbMax(boxVerticesInHfShape[0]); for(PxU32 i=1; i<8; i++) { for(PxU32 dim = 0; dim < 3; dim++) { aabbMin[dim] = PxMin(aabbMin[dim], boxVerticesInHfShape[i][dim]); aabbMax[dim] = PxMax(aabbMax[dim], boxVerticesInHfShape[i][dim]); } } const bool thicknessNegOrNull = (hf.getThicknessFast() <= 0.0f); // PT: don't do this each time! FCMPs are slow. // Compute the height field extreme over the bounds area. // PxReal hfExtreme = thicknessNegOrNull ? -PX_MAX_REAL : PX_MAX_REAL; // PT: we already computed those! // const PxReal oneOverRowScale = 1.0f / shapeHeightField.rowScale; // const PxReal oneOverColumnScale = 1.0f / shapeHeightField.columnScale; const PxReal oneOverRowScale = hfUtil.getOneOverRowScale(); const PxReal oneOverColumnScale = hfUtil.getOneOverColumnScale(); PxU32 minRow; PxU32 maxRow; PxU32 minColumn; PxU32 maxColumn; if (hfGeom.rowScale < 0) { minRow = hf.getMinRow(aabbMax.x * oneOverRowScale); maxRow = hf.getMaxRow(aabbMin.x * oneOverRowScale); } else { minRow = hf.getMinRow(aabbMin.x * oneOverRowScale); maxRow = hf.getMaxRow(aabbMax.x * oneOverRowScale); } if (hfGeom.columnScale < 0) { minColumn = hf.getMinColumn(aabbMax.z * oneOverColumnScale); maxColumn = hf.getMaxColumn(aabbMin.z * oneOverColumnScale); } else { minColumn = hf.getMinColumn(aabbMin.z * oneOverColumnScale); maxColumn = hf.getMaxColumn(aabbMax.z * oneOverColumnScale); } PxReal hfExtreme = hf.computeExtreme(minRow, maxRow, minColumn, maxColumn); //hfExtreme *= hfShape.getHeightScale(); hfExtreme *= hfGeom.heightScale; // Return if convex is on the wrong side of the extreme. if (thicknessNegOrNull) { if (aabbMin.y > hfExtreme) return false; } else { if (aabbMax.y < hfExtreme) return false; } // Test box vertices. if (gCompileBoxVertex) { for(PxU32 i=0; i<8; i++) { const int32_t* tmp = reinterpret_cast(&boxVertexNormalsInHfShape[i].y); // PT: orientation culling if(*tmp>0) // if(PX_SIR(boxVertexNormalsInHfShape[i].y)>0) continue; const PxVec3& boxVertexInHfShape = boxVerticesInHfShape[i]; #if 0 PxVec3 pt = boxShapeAbsPose.transform(boxVertices[i]); PxVec3 worldNormal = boxShapeAbsPose.rotate(boxVertexNormals[i]); //gVisualizeLine(pt, pt+PxVec3(1.0f,0.0f,0.0f), context, PxDebugColor::eARGB_RED); //gVisualizeLine(pt, pt+PxVec3(0.0f,1.0f,0.0f), context, PxDebugColor::eARGB_GREEN); //gVisualizeLine(pt, pt+PxVec3(0.0f,0.0f,1.0f), context, PxDebugColor::eARGB_BLUE); gVisualizeLine(pt, pt+worldNormal, context, PxDebugColor::eARGB_MAGENTA); #endif //////// SAME CODE AS IN CONVEX-HF const bool insideExtreme = thicknessNegOrNull ? (boxVertexInHfShape.y < hfExtreme + params.mContactDistance) : (boxVertexInHfShape.y > hfExtreme-params.mContactDistance); //if (insideExtreme && hfShape.isShapePointOnHeightField(boxVertexInHfShape.x, boxVertexInHfShape.z)) if (insideExtreme && hfUtil.isShapePointOnHeightField(boxVertexInHfShape.x, boxVertexInHfShape.z)) { //PxReal y = hfShape.getHeightAtShapePoint(boxVertexInHfShape.x, boxVertexInHfShape.z); // const PxReal y = hfUtil.getHeightAtShapePoint(boxVertexInHfShape.x, boxVertexInHfShape.z); // PT: compute this once, reuse results (3 times!) // PT: TODO: also reuse this in EE tests PxReal fracX, fracZ; const PxU32 vertexIndex = hfUtil.getHeightField().computeCellCoordinates( boxVertexInHfShape.x * oneOverRowScale, boxVertexInHfShape.z * oneOverColumnScale, fracX, fracZ); const PxReal y = hfUtil.getHeightAtShapePoint2(vertexIndex, fracX, fracZ); const PxReal dy = boxVertexInHfShape.y - y; #ifdef DISTANCE_BASED_TEST if (hf.isDeltaHeightInsideExtent(dy, params.mContactDistance)) #else if (hf.isDeltaHeightInsideExtent(dy)) #endif { const PxU32 faceIndex = hfUtil.getFaceIndexAtShapePointNoTest2(vertexIndex, fracX, fracZ); if (faceIndex != 0xffffffff) { // PxcMaterialIndex material = hfShape.getTriangleMaterial(feature); PxVec3 n; //n = hfShape.getNormalAtShapePoint(boxVertexInHfShape.x, boxVertexInHfShape.z); // n = hfUtil.getNormalAtShapePoint(boxVertexInHfShape.x, boxVertexInHfShape.z); n = hfUtil.getNormalAtShapePoint2(vertexIndex, fracX, fracZ); n = n.getNormalized(); contactBuffer #ifdef DISTANCE_BASED_TEST .contact(boxShapeAbsPose.transform(boxVertices[i]), hfShapeAbsPose.rotate(n), n.y*dy/* + contactDistance*/, faceIndex); #else .contact(boxShapeAbsPose.transform(boxVertices[i]), hfShapeAbsPose.rotate(n), n.y*dy + contactDistance, feature);//add contactDistance to compensate for fact that we don't support dist based contacts! See comment at start of funct. #endif } } } ////////~SAME CODE AS IN CONVEX-HF } } // Test box edges. if (gCompileEdgeEdge) { // create helper class for the trace segment GuContactHeightfieldTraceSegmentHelper traceSegmentHelper(hfUtil); for(PxU32 i=0; i<12; i++) { // PT: orientation culling //float worldNormalY = (boxVertexNormalsInHfShape[edges[2*i]].y + boxVertexNormalsInHfShape[edges[2*i+1]].y)*0.5f; float worldNormalY = boxVertexNormalsInHfShape[edges[2*i]].y + boxVertexNormalsInHfShape[edges[2*i+1]].y; if(worldNormalY>0.0f) continue; const PxVec3& v0 = boxVerticesInHfShape[edges[2*i]]; const PxVec3& v1 = boxVerticesInHfShape[edges[2*i+1]]; #if 0 PxVec3 pt0 = boxShapeAbsPose.transform(boxVertices[edges[2*i]]); PxVec3 pt1 = boxShapeAbsPose.transform(boxVertices[edges[2*i+1]]); PxVec3 worldNormal0 = boxShapeAbsPose.rotate(boxVertexNormals[edges[2*i]]); PxVec3 worldNormal1 = boxShapeAbsPose.rotate(boxVertexNormals[edges[2*i+1]]); PxVec3 pt = (pt0 + pt1)*0.5f; PxVec3 worldNormal = (worldNormal0 + worldNormal1)*0.5f; gVisualizeLine(pt, pt+worldNormal, context, PxDebugColor::eARGB_CYAN); #endif if (hf.getThicknessFast()) // PT: errr...? not the same test as in the convex code? { if ((v0.y > hfExtreme) && (v1.y > hfExtreme)) continue; } else { if ((v0.y < hfExtreme) && (v1.y < hfExtreme)) continue; } GuContactTraceSegmentCallback cb(v1 - v0, contactBuffer, hfShapeAbsPose, params.mContactDistance/*, context.mRenderOutput*/); //context.mRenderOutput << PxVec3(1,0,0) << Gu::Debug::convertToPxMat44(transform1) // << Cm::RenderOutput::LINES << v0+PxVec3(0.01f) << v1+PxVec3(0.01f); //hfShape.traceSegment(v0, v1, &cb); traceSegmentHelper.traceSegment(v0, v1, &cb); } } // Test HeightField vertices. if (gCompileHeightFieldVertex) { // Iterate over all HeightField vertices inside the bounds. for(PxU32 row = minRow; row <= maxRow; row++) { for(PxU32 column = minColumn; column <= maxColumn; column++) { const PxU32 vertexIndex = row * hf.getNbColumnsFast() + column; //if (!hfShape.isCollisionVertex(vertexIndex)) continue; if (!hfUtil.isCollisionVertex(vertexIndex, row, column)) continue; // Check if hf vertex is inside the box. //PxVec3 hfVertex; //hfVertex.set(hfShape.getRowScale() * row, hfShape.getHeightScale() * hfShape.getHeight(vertexIndex), hfShape.getColumnScale() * column); // const PxVec3 hfVertex(shapeHeightField.rowScale * row, shapeHeightField.columnScale * hfShape.getHeight(vertexIndex), shapeHeightField.columnScale * column); const PxVec3 hfVertex(hfGeom.rowScale * row, hfGeom.heightScale * hf.getHeight(vertexIndex), hfGeom.columnScale * column); const PxVec3 hfVertexInBoxShape = boxShape2HfShape.transformTranspose(hfVertex); if ((PxAbs(hfVertexInBoxShape.x) - shapeBox.halfExtents.x - params.mContactDistance < 0) && (PxAbs(hfVertexInBoxShape.y) - shapeBox.halfExtents.y - params.mContactDistance < 0) && (PxAbs(hfVertexInBoxShape.z) - shapeBox.halfExtents.z - params.mContactDistance < 0)) { // ptchernev: should have done this in HeightFieldShape // check if this really is a collision vertex //PxVec3 hfVertexNormal = thicknessNegOrNull ? hfShape.getVertexNormal(vertexIndex) : -hfShape.getVertexNormal(vertexIndex); // PxVec3 hfVertexNormal = thicknessNegOrNull ? hfUtil.getVertexNormal(vertexIndex) : -hfUtil.getVertexNormal(vertexIndex); const PxVec3 nrm = hfUtil.getVertexNormal(vertexIndex, row, column); PxVec3 hfVertexNormal = thicknessNegOrNull ? nrm : -nrm; hfVertexNormal = hfVertexNormal.getNormalized(); const PxVec3 hfVertexNormalInBoxShape = boxShape2HfShape.rotateTranspose(hfVertexNormal); PxVec3 normal; PxReal depth; if (!GuDepenetrateBox(hfVertexInBoxShape, -hfVertexNormalInBoxShape, shapeBox.halfExtents, params.mContactDistance, normal, depth)) { continue; } // PxMat33 rot(boxShape2HfShape[0],boxShape2HfShape[1],boxShape2HfShape[2]); // PxVec3 normalInHfShape = rot * (-normal); PxVec3 normalInHfShape = boxShape2HfShape.rotate(-normal); //hfShape.clipShapeNormalToVertexVoronoi(normalInHfShape, vertexIndex); hfUtil.clipShapeNormalToVertexVoronoi(normalInHfShape, vertexIndex, row, column); if (normalInHfShape.dot(hfVertexNormal) < PX_EPS_REAL) { // hmm, I dont think this can happen continue; } normalInHfShape = normalInHfShape.getNormalized(); const PxU32 faceIndex = hfUtil.getVertexFaceIndex(vertexIndex, row, column); contactBuffer .contact(hfShapeAbsPose.transform(hfVertex), hfShapeAbsPose.rotate(normalInHfShape), #ifdef DISTANCE_BASED_TEST -depth, #else -depth + contactDistance, //add contactDistance to compensate for fact that we don't support dist based contacts! See comment at start of funct. #endif faceIndex); } } } } return contactBuffer.count > 0; } }//Gu }//physx