void ArmModule::draw(Box2f viewport, Box2f screen_viewport, float scale, unsigned int recurse) { glMatrixMode(GL_PROJECTION); glPushMatrix(); glBoxToBox(viewport, screen_viewport); glBegin(GL_QUADS); glColor3f(0.0f, 0.0f, 0.0f); glVertex2f(-1.0f, -0.5f); glVertex2f( 1.0f, -0.5f); glVertex2f( 1.0f, 0.5f); glVertex2f(-1.0f, 0.5f); glEnd(); { //draw arm. glMatrixMode(GL_MODELVIEW); glColor3f(0.5f, 0.5f, 0.5f); glPushMatrix(); glTranslatef(base().x, base().y, 0.0f); glRotatef(base().z / M_PI * 180.0f, 0.0f, 0.0f, 1.0f); capsule(BaseLen, 0.9f); glPopMatrix(); glPushMatrix(); glTranslatef(hand().x, hand().y, 0.0f); glRotatef(hand().z / M_PI * 180.0f, 0.0f, 0.0f, 1.0f); glBegin(GL_QUADS); glVertex2f(0.0f, 0.0f); glVertex2f(1.5f * Rad, 1.5f * Rad); glVertex2f(3.0f * Rad, 0.0f); glVertex2f(1.5f * Rad,-1.5f * Rad); glEnd(); glPopMatrix(); glColor3f(0.7f, 0.7f, 0.7f); glPushMatrix(); glTranslatef(seg().x, seg().y, 0.0f); glRotatef(seg().z / M_PI * 180.0f, 0.0f, 0.0f, 1.0f); capsule(SegLen); glPopMatrix(); glMatrixMode(GL_PROJECTION); } glBegin(GL_LINE_LOOP); glVertex2f(-1.0f, -0.5f); glVertex2f( 1.0f, -0.5f); glVertex2f( 1.0f, 0.5f); glVertex2f(-1.0f, 0.5f); glEnd(); glPopMatrix(); glMatrixMode(GL_MODELVIEW); Graphics::gl_errors("Arm::draw"); }
Trade::MeshData3D Capsule3D::solid(UnsignedInt hemisphereRings, UnsignedInt cylinderRings, UnsignedInt segments, Float halfLength, TextureCoords textureCoords) { CORRADE_ASSERT(hemisphereRings >= 1 && cylinderRings >= 1 && segments >= 3, "Capsule must have at least one hemisphere ring, one cylinder ring and three segments", Trade::MeshData3D(MeshPrimitive::Triangles, std::vector<UnsignedInt>{}, std::vector<std::vector<Vector3>>{}, std::vector<std::vector<Vector3>>{}, std::vector<std::vector<Vector2>>{})); Implementation::Spheroid capsule(segments, textureCoords == TextureCoords::Generate ? Implementation::Spheroid::TextureCoords::Generate : Implementation::Spheroid::TextureCoords::DontGenerate); Float height = 2.0f+2.0f*halfLength; Float hemisphereTextureCoordsVIncrement = 1.0f/(hemisphereRings*height); Rad hemisphereRingAngleIncrement(Constants::pi()/(2*hemisphereRings)); /* Bottom cap vertex */ capsule.capVertex(-height/2, -1.0f, 0.0f); /* Rings of bottom hemisphere */ capsule.hemisphereVertexRings(hemisphereRings-1, -halfLength, -Rad(Constants::pi())/2+hemisphereRingAngleIncrement, hemisphereRingAngleIncrement, hemisphereTextureCoordsVIncrement, hemisphereTextureCoordsVIncrement); /* Rings of cylinder */ capsule.cylinderVertexRings(cylinderRings+1, -halfLength, 2.0f*halfLength/cylinderRings, 1.0f/height, 2.0f*halfLength/(cylinderRings*height)); /* Rings of top hemisphere */ capsule.hemisphereVertexRings(hemisphereRings-1, halfLength, hemisphereRingAngleIncrement, hemisphereRingAngleIncrement, (1.0f + 2.0f*halfLength)/height+hemisphereTextureCoordsVIncrement, hemisphereTextureCoordsVIncrement); /* Top cap vertex */ capsule.capVertex(height/2, 1.0f, 1.0f); /* Faces */ capsule.bottomFaceRing(); capsule.faceRings(hemisphereRings*2-2+cylinderRings); capsule.topFaceRing(); return capsule.finalize(); }
void CapsuleTest::applyTransformation() { Physics::Capsule3D capsule({1.0f, 2.0f, 3.0f}, {-1.0f, -2.0f, -3.0f}, 7.0f); capsule.applyTransformationMatrix(Matrix4::rotation(Deg(90.0f), Vector3::zAxis())); CORRADE_COMPARE(capsule.transformedA(), Vector3(-2.0f, 1.0f, 3.0f)); CORRADE_COMPARE(capsule.transformedB(), Vector3(2.0f, -1.0f, -3.0f)); CORRADE_COMPARE(capsule.transformedRadius(), 7.0f); /* Apply average scaling to radius */ capsule.applyTransformationMatrix(Matrix4::scaling({Constants::sqrt3(), -Constants::sqrt2(), 2.0f})); CORRADE_COMPARE(capsule.transformedRadius(), Constants::sqrt3()*7.0f); }
void display() { /* * A Simple display function to run the entire show * static variables are used to save past state of * the worm, and reflect future motion based on state */ static float trans; static float timer; static float dt; if(dt == 0.0f){ dt = DELTA_T; trans = START_CORD; } glMatrixMode(GL_PROJECTION); glLoadIdentity(); glOrtho(-2.0 * 64/48.0, 2.0 * 64/48.0, -2.0, 2.0, 0.1, 100); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); // Angular (maybe isometric) view gluLookAt(2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); // Side View //gluLookAt(1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); // Top View //gluLookAt(0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); glClear(GL_COLOR_BUFFER_BIT); glColor4f(0,0,1.0,1.0); glTranslatef(trans, 0.0, 0.0); capsule(); glTranslatef(0.5f, 0.0, 0.0); glRotatef(ANGLE_MAX*(timer), 0.0, 0.0, 1.0f); body(); glTranslatef(0.5f, 0.0, 0.0); glRotatef(-2*ANGLE_MAX*(timer), 0.0, 0.0, 1.0f); body(); glTranslatef(0.5f, 0.0, 0.0); glRotatef(ANGLE_MAX*(timer), 0.0, 0.0, 1.0f); body(); if(timer == 1 || (timer==0 && dt<0)){ dt = -dt; } #ifdef __APPLE__ glSwapAPPLE(); #else glSwapBuffers(); #endif trans = trans + 0.1f*timer; timer = timer + dt; }
void CapsuleTest::collisionPoint() { Physics::Capsule3D capsule({-1.0f, -1.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, 2.0f); Physics::Point3D point({2.0f, 0.0f, 0.0f}); Physics::Point3D point1({2.9f, 1.0f, 0.0f}); Physics::Point3D point2({1.0f, 3.1f, 0.0f}); randomTransformation(capsule); randomTransformation(point); randomTransformation(point1); randomTransformation(point2); VERIFY_COLLIDES(capsule, point); VERIFY_COLLIDES(capsule, point1); VERIFY_NOT_COLLIDES(capsule, point2); }
void CapsuleTest::collisionSphere() { Physics::Capsule3D capsule({-1.0f, -1.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, 2.0f); Physics::Sphere3D sphere({3.0f, 0.0f, 0.0f}, 0.9f); Physics::Sphere3D sphere1({3.5f, 1.0f, 0.0f}, 0.6f); Physics::Sphere3D sphere2({1.0f, 4.1f, 0.0f}, 1.0f); randomTransformation(capsule); randomTransformation(sphere); randomTransformation(sphere1); randomTransformation(sphere2); VERIFY_COLLIDES(capsule, sphere); VERIFY_COLLIDES(capsule, sphere1); VERIFY_NOT_COLLIDES(capsule, sphere2); }
double capsuleBoxDistance(const Vector3d& a_start, const Vector3d& a_end, const double a_radius, const Vector3d& b_center, const Vector3d& b_half_length, Vector3d& direction) { btTransform tr[2]; Matrix3d rotationA; rotation_from_tangent((a_start - a_end).normalized(), rotationA); btMatrix3x3 basisA; basisA.setValue(rotationA(0,1), rotationA(0,0), rotationA(0,2), rotationA(1,1), rotationA(1,0), rotationA(1,2), rotationA(2,1), rotationA(2,0), rotationA(2,2)); btMatrix3x3 basisB; basisB.setIdentity(); tr[0].setBasis(basisA); tr[1].setBasis(basisB); Vector3d mid_point = (a_start + a_end)/2.0; btVector3 originA(mid_point(0), mid_point(1), mid_point(2)); btVector3 originB(b_center(0), b_center(1), b_center(2)); tr[0].setOrigin(originA); tr[1].setOrigin(originB); btCollisionShape* shapePtr[2]; btCapsuleShape capsule(btScalar(a_radius), btScalar((a_start-a_end).norm())); btBoxShape box(btVector3(b_half_length(0), b_half_length(1), b_half_length(2))); shapePtr[0] = &capsule; shapePtr[1] = &box; btDefaultCollisionConfiguration collisionConfiguration; btCollisionDispatcher dispatcher(&collisionConfiguration); btDbvtBroadphase pairCache; btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration); world.getDispatchInfo().m_convexMaxDistanceUseCPT = true; MyContactResultCallback result; btCollisionObject obA; obA.setCollisionShape(shapePtr[0]); obA.setWorldTransform(tr[0]); btCollisionObject obB; obB.setCollisionShape(shapePtr[1]); obB.setWorldTransform(tr[1]); world.contactPairTest(&obA,&obB,result); direction = result.positionWorldOnB - result.positionWorldOnA; return result.distance; }
void KinematicController::CreatePhyiscsAgent() { PxTransform transform(PxVec3(m_position.x, m_position.y, m_position.z)); //transform = PxVec3(0, 0, 0); float density = 50; float halfHeight = m_height / 2; PxCapsuleGeometry capsule(m_radius, m_height); m_actor = PxCreateDynamic(*m_physicsObject->m_Physics, transform, capsule, *m_physicsObject->m_PhysicsMaterial, density); m_physicsObject->m_PhysicsScene->addActor(*m_actor); m_physicsObject->m_boxActors.push_back(m_actor); }
/*static*/ VtValue UsdImagingCapsuleAdapter::GetMeshPoints(UsdPrim const& prim, UsdTimeCode time) { UsdGeomCapsule capsule(prim); double radius = 0.5; double height = 1.0; TfToken axis = UsdGeomTokens->z; TF_VERIFY(capsule.GetRadiusAttr().Get(&radius, time)); TF_VERIFY(capsule.GetHeightAttr().Get(&height, time)); TF_VERIFY(capsule.GetAxisAttr().Get(&axis, time)); // We can't express varying radius and height via a non-uniform // scaling transformation and maintain spherical end caps. return VtValue(_GenerateCapsuleMeshPoints(float(radius), float(height), axis)); }
osg::ref_ptr<osg::Node> get(const CapsuleVec_t& capsules) { osg::ref_ptr<osg::Group> pAddToThisGroup(new osg::Group()); for ( const auto& cc : capsules ) { osg::Vec3d pp( cc.begin - cc.end ); double height( pp.length() ); if ( height < 0.000001 ) continue; osg::Vec3d center( (cc.begin.x() + cc.end.x())/2.0, (cc.begin.y() + cc.end.y())/2.0, (cc.begin.z() + cc.end.z())/2.0 ); // This is the default direction for the capsules to face in OpenGL static const osg::Vec3d ZZ( 0,0,1 ); // Get CROSS product (the axis of rotation) osg::Vec3d tt( ZZ^pp ); // Get angle. length is magnitude of the vector double angle( acos( (ZZ * pp) / height) ); // Create a capsule between the two points with the given radius osg::ref_ptr<osg::Capsule> capsule( new osg::Capsule(center, cc.radius, height) ); capsule->setRotation(osg::Quat(angle, osg::Vec3d(tt.x(), tt.y(), tt.z()))); // A geode to hold the capsule osg::ref_ptr<osg::ShapeDrawable> capsuleDrawable( new osg::ShapeDrawable(capsule) ); capsuleDrawable->setColor(cc.color); osg::ref_ptr<osg::Geode> geode( new osg::Geode() ); geode->addDrawable(capsuleDrawable); // Set the color of the capsule that extends between the two points. // osg::ref_ptr<osg::Material> pMaterial( new osg::Material() ); // pMaterial->setDiffuse( osg::Material::FRONT, cc.color); // geode->getOrCreateStateSet()->setAttribute( pMaterial, osg::StateAttribute::OVERRIDE ); geode->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); geode->getOrCreateStateSet()->setRenderingHint(osg::StateSet::TRANSPARENT_BIN); pAddToThisGroup->addChild(geode); } return pAddToThisGroup; }
Trade::MeshData3D Capsule3D::wireframe(const UnsignedInt hemisphereRings, const UnsignedInt cylinderRings, const UnsignedInt segments, const Float halfLength) { CORRADE_ASSERT(hemisphereRings >= 1 && cylinderRings >= 1 && segments >= 4 && segments%4 == 0, "Primitives::Capsule::wireframe(): improper parameters", Trade::MeshData3D(MeshPrimitive::Lines, std::vector<UnsignedInt>{}, std::vector<std::vector<Vector3>>{}, std::vector<std::vector<Vector3>>{}, std::vector<std::vector<Vector2>>{})); Implementation::WireframeSpheroid capsule(segments/4); /* Bottom hemisphere */ capsule.bottomHemisphere(-halfLength, hemisphereRings); /* Cylinder */ capsule.ring(-halfLength); for(UnsignedInt i = 0; i != cylinderRings; ++i) { capsule.cylinder(); capsule.ring(-halfLength + (i+1)*(2.0f*halfLength/cylinderRings)); } /* Top hemisphere */ capsule.topHemisphere(halfLength, hemisphereRings); return capsule.finalize(); }
void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_tolerance, S test_tolerance) { using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>; // Capsule<S> of radius 2 and of height 4 CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule<S> (2., 4.)); // Box<S> of size 1 by 2 by 4 CollisionGeometryPtr_t boxGeometry (new fcl::Box<S> (1., 2., 4.)); // Enable computation of nearest points fcl::DistanceRequest<S> distanceRequest (true); fcl::DistanceResult<S> distanceResult; distanceRequest.gjk_solver_type = solver_type; distanceRequest.distance_tolerance = solver_tolerance; // Rotate capsule around y axis by pi/2 and move it behind box fcl::Transform3<S> tf1 = fcl::Translation3<S>(fcl::Vector3<S>(-10., 0.8, 1.5)) *fcl::Quaternion<S>(sqrt(2)/2, 0, sqrt(2)/2, 0); fcl::Transform3<S> tf2 = fcl::Transform3<S>::Identity(); fcl::CollisionObject<S> capsule (capsuleGeometry, tf1); fcl::CollisionObject<S> box (boxGeometry, tf2); // test distance distanceResult.clear (); fcl::distance (&capsule, &box, distanceRequest, distanceResult); fcl::Vector3<S> o1 = distanceResult.nearest_points [0]; fcl::Vector3<S> o2 = distanceResult.nearest_points [1]; EXPECT_NEAR (distanceResult.min_distance, 5.5, test_tolerance); EXPECT_NEAR (o1 [0], -6.0, test_tolerance); EXPECT_NEAR (o1 [1], 0.8, test_tolerance); EXPECT_NEAR (o1 [2], 1.5, test_tolerance); EXPECT_NEAR (o2 [0], -0.5, test_tolerance); EXPECT_NEAR (o2 [1], 0.8, test_tolerance); EXPECT_NEAR (o2 [2], 1.5, test_tolerance); }
void createScenePrimitives() { // sphere { int id = numRigidBodies++; PfxSphere sphere(1.0f); PfxShape shape; shape.reset(); shape.setSphere(sphere); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(1.0f); bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f)); states[id].reset(); states[id].setPosition(PfxVector3(-5.0f,5.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // box { int id = numRigidBodies++; PfxBox box(1.0f,1.0f,1.0f); PfxShape shape; shape.reset(); shape.setBox(box); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(1.0f); bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(1.0f),1.0f)); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,5.0f,5.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // capsule { int id = numRigidBodies++; PfxCapsule capsule(1.5f,0.5f); PfxShape shape; shape.reset(); shape.setCapsule(capsule); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(2.0f); bodies[id].setInertia(pfxCalcInertiaCylinderX(2.0f,0.5f,2.0f)); states[id].reset(); states[id].setPosition(PfxVector3(5.0f,5.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // cylinder { int id = numRigidBodies++; PfxCylinder cylinder(0.5f,1.5f); PfxShape shape; shape.reset(); shape.setCylinder(cylinder); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(3.0f); bodies[id].setInertia(pfxCalcInertiaCylinderX(0.5f,1.5f,3.0f)); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,10.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // convex mesh { PfxCreateConvexMeshParam param; param.verts = BarrelVtx; param.numVerts = BarrelVtxCount; param.vertexStrideBytes = sizeof(float)*6; param.triangles = BarrelIdx; param.numTriangles = BarrelIdxCount/3; param.triangleStrideBytes = sizeof(unsigned short)*3; PfxInt32 ret = pfxCreateConvexMesh(gConvex,param); if(ret != SCE_PFX_OK) { SCE_PFX_PRINTF("Can't create gConvex mesh.\n"); } int id = numRigidBodies++; PfxShape shape; shape.reset(); shape.setConvexMesh(&gConvex); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(3.0f); bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f)); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,15.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // combined primitives { int id = numRigidBodies++; //E Both shapes and incides buffer have to be kept when creating a combined shape. static PfxShape shapes[3]; PfxUInt16 shapeIds[3]={0,1,2}; collidables[id].reset(shapes,shapeIds,3); { PfxBox box(0.5f,0.5f,1.5f); PfxShape shape; shape.reset(); shape.setBox(box); shape.setOffsetPosition(PfxVector3(-2.0f,0.0f,0.0f)); collidables[id].addShape(shape); } { PfxBox box(0.5f,1.5f,0.5f); PfxShape shape; shape.reset(); shape.setBox(box); shape.setOffsetPosition(PfxVector3(2.0f,0.0f,0.0f)); collidables[id].addShape(shape); } { PfxCapsule cap(1.5f,0.5f); PfxShape shape; shape.reset(); shape.setCapsule(cap); collidables[id].addShape(shape); } collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(3.0f); bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(2.5f,1.0f,1.0f),3.0f)); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,5.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } }
PxArticulation* Physics::makeRagdoll(PxPhysics* g_Physics, RagdollNode** nodeArray, PxTransform worldPos, float scaleFactor, PxMaterial* ragdollMaterial) { //create the articulation for our ragdoll PxArticulation *articulation = g_Physics->createArticulation(); RagdollNode** currentNode = nodeArray; //while there are more nodes to process... while (*currentNode != NULL) { //get a pointer to the current node RagdollNode* currentNodePtr = *currentNode; //create a pointer ready to hold the parent node pointer if there is one RagdollNode* parentNode = nullptr; //get scaled values for capsule float radius = currentNodePtr->radius*scaleFactor; float halfLength = currentNodePtr->halfLength*scaleFactor; float childHalfLength = radius + halfLength; float parentHalfLength = 0; //will be set later if there is a parnet //get a pointer to the parent PxArticulationLink* parentLinkPtr = NULL; currentNodePtr->scaledGlobalPos = worldPos.p; if (currentNodePtr->parentNodeIdx != -1) { //if there is a parent then we need to work out our local position for the link //get a pointer to the parent node parentNode = *(nodeArray + currentNodePtr->parentNodeIdx); //get a pointer to the link for the parent parentLinkPtr = parentNode->linkPtr; parentHalfLength = (parentNode->radius + parentNode->halfLength) *scaleFactor; //work out the local position of the node PxVec3 currentRelative = currentNodePtr->childLinkPos * currentNodePtr->globalRotation.rotate(PxVec3(childHalfLength, 0, 0)); PxVec3 parentRelative = -currentNodePtr->parentLinkPos * parentNode->globalRotation.rotate(PxVec3(parentHalfLength, 0, 0)); currentNodePtr->scaledGlobalPos = parentNode->scaledGlobalPos - (parentRelative + currentRelative); } //build the ransform for the link PxTransform linkTransform = PxTransform(currentNodePtr->scaledGlobalPos, currentNodePtr->globalRotation); //create the link in the articulation PxArticulationLink* link = articulation->createLink(parentLinkPtr, linkTransform); //add the pointer to this link into the ragdoll data so we have it for later when we want to link to it currentNodePtr->linkPtr = link; float jointSpace = 0.01f; //gap between joints float capsuleHalfLength = (halfLength > jointSpace ? halfLength - jointSpace : 0) + 0.01f; PxCapsuleGeometry capsule(radius, capsuleHalfLength); link->createShape(capsule, *ragdollMaterial); //adds a capsule collider to the link PxRigidBodyExt::updateMassAndInertia(*link, 50.0f); //adds some mass, mass should really be part of the data! if (currentNodePtr->parentNodeIdx != -1) { //get the pointer to the joint from the link PxArticulationJoint* joint = link->getInboundJoint(); //get the relative rotation of this link PxQuat frameRotation = parentNode->globalRotation.getConjugate() * currentNodePtr->globalRotation; //set the parent constraint frame PxTransform parentConstraintFrame = PxTransform(PxVec3(currentNodePtr->parentLinkPos*parentHalfLength, 0, 0), frameRotation); //set the child constraint frame (this the constraint frame of the newly added link) PxTransform thisConstraintFrame = PxTransform(PxVec3(currentNodePtr->childLinkPos*childHalfLength, 0, 0)); //set up the poses for the joint so it is in the correct place joint->setParentPose(parentConstraintFrame); joint->setChildPose(thisConstraintFrame); //set up some constraints to stop it flopping around joint->setStiffness(20); joint->setDamping(20); joint->setSwingLimit(currentNodePtr->ySwingLimit, currentNodePtr->zSwingLimit); joint->setSwingLimitEnabled(true); joint->setTwistLimit(-0.1f, 0.1f); joint->setTwistLimitEnabled(true); } currentNode++; } return articulation; }
PhysicsCollisionShape::Definition* PhysicsCollisionShape::Definition::create(Node* node, Properties* properties) { GP_ASSERT(node); // Check if the properties is valid and has a valid namespace. if (!properties || !(strcmp(properties->getNamespace(), "collisionObject") == 0)) { GP_ERROR("Failed to load physics collision shape from properties object: must be non-null object and have namespace equal to 'collisionObject'."); return NULL; } // Set values to their defaults. PhysicsCollisionShape::Type type = PhysicsCollisionShape::SHAPE_BOX; Vector3* extents = NULL; Vector3* center = NULL; float radius = -1.0f; float height = -1.0f; bool centerIsAbsolute = false; const char* imagePath = NULL; bool shapeSpecified = false; // Load the defined properties. properties->rewind(); const char* name; while (name = properties->getNextProperty()) { if (strcmp(name, "shape") == 0) { std::string shapeStr = properties->getString(); if (shapeStr == "BOX") type = SHAPE_BOX; else if (shapeStr == "SPHERE") type = SHAPE_SPHERE; else if (shapeStr == "MESH") type = SHAPE_MESH; else if (shapeStr == "HEIGHTFIELD") type = SHAPE_HEIGHTFIELD; else if (shapeStr == "CAPSULE") type = SHAPE_CAPSULE; else { GP_ERROR("Could not create physics collision shape; unsupported value for collision shape type: '%s'.", shapeStr.c_str()); return NULL; } shapeSpecified = true; } else if (strcmp(name, "image") == 0) { imagePath = properties->getString(); } else if (strcmp(name, "radius") == 0) { radius = properties->getFloat(); } else if (strcmp(name, "height") == 0) { height = properties->getFloat(); } else if (strcmp(name, "extents") == 0) { extents = new Vector3(); properties->getVector3("extents", extents); } else if (strcmp(name, "center") == 0) { center = new Vector3(); properties->getVector3("center", center); } else if (strcmp(name, "centerAbsolute") == 0) { centerIsAbsolute = properties->getBool(); } else { // Ignore this case (these are the properties for the rigid body, character, or ghost object that this collision shape is for). } } if (!shapeSpecified) { GP_ERROR("Missing 'shape' specifier for collision shape definition."); return NULL; } // Create the collision shape. PhysicsCollisionShape::Definition* shape = new PhysicsCollisionShape::Definition(); switch (type) { case SHAPE_BOX: if (extents) { if (center) { *shape = box(*extents, *center, centerIsAbsolute); } else { *shape = box(*extents); } } else { *shape = box(); } break; case SHAPE_SPHERE: if (radius != -1.0f) { if (center) { *shape = sphere(radius, *center, centerIsAbsolute); } else { *shape = sphere(radius); } } else { *shape = sphere(); } break; case SHAPE_CAPSULE: if (radius != -1.0f && height != -1.0f) { if (center) { *shape = capsule(radius, height, *center, centerIsAbsolute); } else { *shape = capsule(radius, height); } } else { *shape = capsule(); } break; case SHAPE_MESH: { // Mesh is required on node. Mesh* nodeMesh = node->getModel() ? node->getModel()->getMesh() : NULL; if (nodeMesh == NULL) { GP_ERROR("Cannot create mesh collision object for node without model/mesh."); return NULL; } // Check that the node's mesh's primitive type is supported. switch (nodeMesh->getPrimitiveType()) { case Mesh::TRIANGLES: { *shape = mesh(nodeMesh); break; } case Mesh::LINES: case Mesh::LINE_STRIP: case Mesh::POINTS: case Mesh::TRIANGLE_STRIP: GP_ERROR("Mesh collision objects are currently only supported on meshes with primitive type equal to TRIANGLES."); SAFE_DELETE(shape); break; } break; } case SHAPE_HEIGHTFIELD: if (imagePath == NULL) { GP_ERROR("Heightfield collision objects require an image path."); SAFE_DELETE(shape); return NULL; } else { // Load the image data from the given file path. Image* image = Image::create(imagePath); if (!image) { GP_ERROR("Failed create image for heightfield collision object from file '%s'.", imagePath); SAFE_DELETE(shape); return NULL; } // Ensure that the image's pixel format is supported. switch (image->getFormat()) { case Image::RGB: case Image::RGBA: break; default: GP_ERROR("Heightmap: pixel format is not supported: %d.", image->getFormat()); SAFE_RELEASE(image); SAFE_DELETE(shape); return NULL; } *shape = PhysicsCollisionShape::heightfield(image); SAFE_RELEASE(image); } break; default: GP_ERROR("Unsupported physics collision shape type (%d).", type); SAFE_DELETE(shape); return NULL; } SAFE_DELETE(extents); SAFE_DELETE(center); return shape; }
PhysicsCollisionShape::Definition PhysicsCollisionShape::Definition::create(Node* node, Properties* properties) { GP_ASSERT(node); // Check if the properties is valid and has a valid namespace. if (!properties || !(strcmp(properties->getNamespace(), "collisionObject") == 0)) { GP_ERROR("Failed to load physics collision shape from properties object: must be non-null object and have namespace equal to 'collisionObject'."); return Definition(); } // Set values to their defaults. PhysicsCollisionShape::Type type = PhysicsCollisionShape::SHAPE_BOX; Vector3 extents, center; bool extentsSpecified = false; bool centerSpecified = false; float radius = -1.0f; float width = -1.0f; float height = -1.0f; bool centerIsAbsolute = false; const char* imagePath = NULL; float maxHeight = 0; float minHeight = 0; bool shapeSpecified = false; // Load the defined properties. properties->rewind(); const char* name; while (name = properties->getNextProperty()) { if (strcmp(name, "shape") == 0) { std::string shapeStr = properties->getString(); if (shapeStr == "BOX") type = SHAPE_BOX; else if (shapeStr == "SPHERE") type = SHAPE_SPHERE; else if (shapeStr == "MESH") type = SHAPE_MESH; else if (shapeStr == "HEIGHTFIELD") type = SHAPE_HEIGHTFIELD; else if (shapeStr == "CAPSULE") type = SHAPE_CAPSULE; else { GP_ERROR("Could not create physics collision shape; unsupported value for collision shape type: '%s'.", shapeStr.c_str()); return Definition(); } shapeSpecified = true; } else if (strcmp(name, "image") == 0) { imagePath = properties->getString(); } else if (strcmp(name, "maxHeight") == 0) { maxHeight = properties->getFloat(); } else if (strcmp(name, "minHeight") == 0) { minHeight = properties->getFloat(); } else if (strcmp(name, "radius") == 0) { radius = properties->getFloat(); } else if (strcmp(name, "width") == 0) { width = properties->getFloat(); } else if (strcmp(name, "height") == 0) { height = properties->getFloat(); } else if (strcmp(name, "extents") == 0) { properties->getVector3("extents", &extents); extentsSpecified = true; } else if (strcmp(name, "center") == 0) { properties->getVector3("center", ¢er); centerSpecified = true; } else if (strcmp(name, "centerAbsolute") == 0) { centerIsAbsolute = properties->getBool(); } else { // Ignore this case (these are the properties for the rigid body, character, or ghost object that this collision shape is for). } } if (!shapeSpecified) { GP_ERROR("Missing 'shape' specifier for collision shape definition."); return Definition(); } // Create the collision shape. Definition shape; switch (type) { case SHAPE_BOX: if (extentsSpecified) { if (centerSpecified) { shape = box(extents, center, centerIsAbsolute); } else { shape = box(extents); } } else { shape = box(); } break; case SHAPE_SPHERE: if (radius != -1.0f) { if (centerSpecified) { shape = sphere(radius, center, centerIsAbsolute); } else { shape = sphere(radius); } } else { shape = sphere(); } break; case SHAPE_CAPSULE: if (radius != -1.0f && height != -1.0f) { if (centerSpecified) { shape = capsule(radius, height, center, centerIsAbsolute); } else { shape = capsule(radius, height); } } else { shape = capsule(); } break; case SHAPE_MESH: { // Mesh is required on node. Mesh* nodeMesh = node->getModel() ? node->getModel()->getMesh() : NULL; if (nodeMesh == NULL) { GP_ERROR("Cannot create mesh collision object for node without model/mesh."); } else { // Check that the node's mesh's primitive type is supported. switch (nodeMesh->getPrimitiveType()) { case Mesh::TRIANGLES: shape = mesh(nodeMesh); break; case Mesh::LINES: case Mesh::LINE_STRIP: case Mesh::POINTS: case Mesh::TRIANGLE_STRIP: GP_ERROR("Mesh collision objects are currently only supported on meshes with primitive type equal to TRIANGLES."); break; } } } break; case SHAPE_HEIGHTFIELD: { if (imagePath == NULL) { // Node requires a valid terrain if (node->getTerrain() == NULL) { GP_ERROR("Heightfield collision objects can only be specified on nodes that have a valid terrain, or that specify an image path."); } else { shape = PhysicsCollisionShape::heightfield(); } } else { std::string ext = FileSystem::getExtension(imagePath); HeightField* heightfield = NULL; if (ext == ".PNG") heightfield = HeightField::createFromImage(imagePath, minHeight, maxHeight); else if (ext == ".RAW" || ext == ".R16") heightfield = HeightField::createFromRAW(imagePath, (unsigned int)width, (unsigned int)height, minHeight, maxHeight); if (heightfield) { shape = PhysicsCollisionShape::heightfield(heightfield); SAFE_RELEASE(heightfield); } } } break; default: GP_ERROR("Unsupported physics collision shape type (%d).", type); break; } return shape; }
void test_distance_capsule_box() { using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>; // Capsule of radius 2 and of height 4 CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule<S> (2., 4.)); // Box of size 1 by 2 by 4 CollisionGeometryPtr_t boxGeometry (new fcl::Box<S> (1., 2., 4.)); // Enable computation of nearest points fcl::DistanceRequest<S> distanceRequest (true); fcl::DistanceResult<S> distanceResult; fcl::Transform3<S> tf1(fcl::Translation3<S>(fcl::Vector3<S> (3., 0, 0))); fcl::Transform3<S> tf2 = fcl::Transform3<S>::Identity(); fcl::CollisionObject<S> capsule (capsuleGeometry, tf1); fcl::CollisionObject<S> box (boxGeometry, tf2); // test distance fcl::distance (&capsule, &box, distanceRequest, distanceResult); // Nearest point on capsule fcl::Vector3<S> o1 (distanceResult.nearest_points [0]); // Nearest point on box fcl::Vector3<S> o2 (distanceResult.nearest_points [1]); EXPECT_NEAR (distanceResult.min_distance, 0.5, 1e-4); EXPECT_NEAR (o1 [0], -2.0, 1e-4); EXPECT_NEAR (o1 [1], 0.0, 1e-4); EXPECT_NEAR (o2 [0], 0.5, 1e-4); EXPECT_NEAR (o1 [1], 0.0, 1e-4); // TODO(JS): maybe o2 rather than o1? // Move capsule above box tf1 = fcl::Translation3<S>(fcl::Vector3<S> (0., 0., 8.)); capsule.setTransform (tf1); // test distance distanceResult.clear (); fcl::distance (&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points [0]; o2 = distanceResult.nearest_points [1]; EXPECT_NEAR (distanceResult.min_distance, 2.0, 1e-4); EXPECT_NEAR (o1 [0], 0.0, 1e-4); EXPECT_NEAR (o1 [1], 0.0, 1e-4); EXPECT_NEAR (o1 [2], -4.0, 1e-4); // Disabled broken test lines. Please see #25. // CHECK_CLOSE_TO_0 (o2 [0], 1e-4); EXPECT_NEAR (o2 [1], 0.0, 1e-4); EXPECT_NEAR (o2 [2], 2.0, 1e-4); // Rotate capsule around y axis by pi/2 and move it behind box tf1.translation() = fcl::Vector3<S>(-10., 0., 0.); tf1.linear() = fcl::Quaternion<S>(sqrt(2)/2, 0, sqrt(2)/2, 0).toRotationMatrix(); capsule.setTransform (tf1); // test distance distanceResult.clear (); fcl::distance (&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points [0]; o2 = distanceResult.nearest_points [1]; EXPECT_NEAR (distanceResult.min_distance, 5.5, 1e-4); EXPECT_NEAR (o1 [0], 0.0, 1e-4); EXPECT_NEAR (o1 [1], 0.0, 1e-4); EXPECT_NEAR (o1 [2], 4.0, 1e-4); EXPECT_NEAR (o2 [0], -0.5, 1e-4); EXPECT_NEAR (o2 [1], 0.0, 1e-4); EXPECT_NEAR (o2 [2], 0.0, 1e-4); }
static void convertShape(btCollisionShape* bulletShape, btAlignedObjectArray<sce::PhysicsEffects::PfxShape>& shapes) { switch (bulletShape->getShapeType()) { case BOX_SHAPE_PROXYTYPE: { btBoxShape* boxshape = (btBoxShape*)bulletShape; sce::PhysicsEffects::PfxBox box(boxshape->getHalfExtentsWithMargin().getX(),boxshape->getHalfExtentsWithMargin().getY(),boxshape->getHalfExtentsWithMargin().getZ()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setBox(box); break; } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) bulletShape; int numSubParts = trimesh->getMeshInterface()->getNumSubParts(); btAssert(numSubParts>0); for (int i=0;i<numSubParts;i++) { unsigned char* vertexBase=0; int numVerts = 0; PHY_ScalarType vertexType; int vertexStride=0; unsigned char* indexBase=0; int indexStride=0; int numFaces=0; PHY_ScalarType indexType; trimesh->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numVerts,vertexType,vertexStride,&indexBase,indexStride,numFaces,indexType,i); sce::PhysicsEffects::PfxCreateLargeTriMeshParam param; btAssert(param.flag&SCE_PFX_MESH_FLAG_16BIT_INDEX); unsigned short int* copyIndices = new unsigned short int[numFaces*3]; switch (indexType) { case PHY_UCHAR: { for (int p=0;p<numFaces;p++) { copyIndices[p*3]=indexBase[p*indexStride]; copyIndices[p*3+1]=indexBase[p*indexStride+1]; copyIndices[p*3+2]=indexBase[p*indexStride+2]; } break; } //PHY_INTEGER: //PHY_SHORT: default: { btAssert(0); } }; param.verts = (float*)vertexBase; param.numVerts = numVerts; param.vertexStrideBytes = vertexStride; param.triangles = copyIndices; param.numTriangles = numFaces; param.triangleStrideBytes = sizeof(unsigned short int)*3; sce::PhysicsEffects::PfxLargeTriMesh* largeMesh = new sce::PhysicsEffects::PfxLargeTriMesh(); sce::PhysicsEffects::PfxInt32 ret = pfxCreateLargeTriMesh(*largeMesh,param); if(ret != SCE_PFX_OK) { SCE_PFX_PRINTF("Can't create large mesh.\n"); } sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setLargeTriMesh(largeMesh); } break; } case SPHERE_SHAPE_PROXYTYPE: { btSphereShape* sphereshape = (btSphereShape*)bulletShape; sce::PhysicsEffects::PfxSphere sphere(sphereshape->getRadius()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setSphere(sphere); break; } case CAPSULE_SHAPE_PROXYTYPE: { btCapsuleShape* capsuleshape= (btCapsuleShape*)bulletShape;//assume btCapsuleShapeX for now sce::PhysicsEffects::PfxCapsule capsule(capsuleshape->getHalfHeight(),capsuleshape->getRadius()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setCapsule(capsule); break; } case CYLINDER_SHAPE_PROXYTYPE: { btCylinderShape* cylindershape= (btCylinderShape*)bulletShape;//assume btCylinderShapeX for now sce::PhysicsEffects::PfxCylinder cylinder(cylindershape->getHalfExtentsWithMargin()[0],cylindershape->getRadius()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setCylinder(cylinder); break; } case CONVEX_HULL_SHAPE_PROXYTYPE: { btConvexHullShape* convexHullShape = (btConvexHullShape*)bulletShape; sce::PhysicsEffects::PfxConvexMesh* convex = new sce::PhysicsEffects::PfxConvexMesh(); convex->m_numVerts = convexHullShape->getNumPoints(); convex->m_numIndices = 0;//todo for ray intersection test support for (int i=0;i<convex->m_numVerts;i++) { convex->m_verts[i].setX(convexHullShape->getPoints()[i].getX()); convex->m_verts[i].setY(convexHullShape->getPoints()[i].getY()); convex->m_verts[i].setZ(convexHullShape->getPoints()[i].getZ()); } convex->updateAABB(); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setConvexMesh(convex); break; } case COMPOUND_SHAPE_PROXYTYPE: { btCompoundShape* compound = (btCompoundShape*) bulletShape; for (int s=0;s<compound->getNumChildShapes();s++) { convertShape(compound->getChildShape(s),shapes); sce::PhysicsEffects::PfxMatrix3 rotMat = getVmMatrix3(compound->getChildTransform(s).getBasis()); sce::PhysicsEffects::PfxVector3 translate = getVmVector3(compound->getChildTransform(s).getOrigin()); sce::PhysicsEffects::PfxTransform3 childtransform(rotMat,translate); shapes[shapes.size()-1].setOffsetTransform(childtransform); } break; } default: { btAssert(0); } }; }
bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS) { PX_UNUSED(renderOutput); const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>(); const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>(); PersistentContactManifold& manifold = cache.getManifold(); Ps::prefetchLine(shapeConvex.hullData); PX_ASSERT(transform1.q.isSane()); PX_ASSERT(transform0.q.isSane()); const Vec3V zeroV = V3Zero(); const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale const FloatV contactDist = FLoad(params.mContactDistance); const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight); const FloatV capsuleRadius = FLoad(shapeCapsule.radius); const ConvexHullData* hullData =shapeConvex.hullData; //Transfer A into the local space of B const PsTransformV transf0 = loadTransformA(transform0); const PsTransformV transf1 = loadTransformA(transform1); const PsTransformV curRTrans(transf1.transformInv(transf0)); const PsMatTransformV aToB(curRTrans); const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale); const FloatV capsuleMinMargin = Gu::CalculateCapsuleMinMargin(capsuleRadius); const FloatV minMargin = FMin(convexMargin, capsuleMinMargin); const PxU32 initialContacts = manifold.mNumContacts; const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(1.25f)); const FloatV refreshDist = FAdd(contactDist, capsuleRadius); manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDist); //ML: after refreshContactPoints, we might lose some contacts const bool bLostContacts = (manifold.mNumContacts != initialContacts); GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT; Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b const FloatV zero = FZero(); FloatV penDep = zero; PX_UNUSED(bLostContacts); if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin)) { const bool idtScale = shapeConvex.scale.isIdentity(); manifold.setRelativeTransform(curRTrans); const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x); ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale); convexHull.setMargin(zero); //transform capsule(a) into the local space of convexHull(b) CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius); LocalConvex<CapsuleV> convexA(capsule); const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter()); if(idtScale) { LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull)); status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true); } else { LocalConvex<ConvexHullV> convexB(convexHull); status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true); } Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts); bool doOverlapTest = false; if(status == GJK_NON_INTERSECT) { return false; } else if(status == GJK_DEGENERATE) { return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal, closestB, convexHull.getMargin(), contactDist, true, renderOutput, FLoad(params.mToleranceLength)); } else { const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f)); if(status == GJK_CONTACT) { const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA); const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep); //Add contact to contact stream manifoldContacts[0].mLocalPointA = localPointA; manifoldContacts[0].mLocalPointB = closestB; manifoldContacts[0].mLocalNormalPen = localNormalPen; //Add contact to manifold manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold); } else { PX_ASSERT(status == EPA_CONTACT); if(idtScale) { LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull)); status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, closestA, closestB, normal, penDep, true); } else { LocalConvex<ConvexHullV> convexB(convexHull); status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, closestA, closestB, normal, penDep, true); } if(status == EPA_CONTACT) { const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA); const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep); //Add contact to contact stream manifoldContacts[0].mLocalPointA = localPointA; manifoldContacts[0].mLocalPointB = closestB; manifoldContacts[0].mLocalNormalPen = localNormalPen; //Add contact to manifold manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold); } else { doOverlapTest = true; } } if(initialContacts == 0 || bLostContacts || doOverlapTest) { return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal, closestB, convexHull.getMargin(), contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength)); } else { //This contact is either come from GJK or EPA normal = transf1.rotate(normal); manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist); #if PCM_LOW_LEVEL_DEBUG manifold.drawManifold(*renderOutput, transf0, transf1); #endif return true; } } } else if (manifold.getNumContacts() > 0) { normal = manifold.getWorldNormal(transf1); manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist); #if PCM_LOW_LEVEL_DEBUG manifold.drawManifold(*renderOutput, transf0, transf1); #endif return true; } return false; }