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;
}
