btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; }
void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); btScalar radius = halfExtents.length(); btVector3 center = (aabbMax + aabbMin) * btScalar(0.5); //this is where the triangles are generated, given AABB and plane equation (normal/constant) btVector3 tangentDir0,tangentDir1; //tangentDir0/tangentDir1 can be precalculated btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1); btVector3 supVertex0,supVertex1; btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; btVector3 triangle[3]; triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius; triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius; triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius; callback->processTriangle(triangle,0,0); triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius; triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius; triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius; callback->processTriangle(triangle,0,1); }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0),m_limit() { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); }
static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( btVector3& normalOnB, btVector3& pointOnB, btScalar capsuleLengthA, btScalar capsuleRadiusA, btScalar capsuleLengthB, btScalar capsuleRadiusB, int capsuleAxisA, int capsuleAxisB, const btTransform& transformA, const btTransform& transformB, btScalar distanceThreshold) { btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA); btVector3 translationA = transformA.getOrigin(); btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB); btVector3 translationB = transformB.getOrigin(); // translation between centers btVector3 translation = translationB - translationA; // compute the closest points of the capsule line segments btVector3 ptsVector; // the vector between the closest points btVector3 offsetA, offsetB; // offsets from segment centers to their closest points btScalar tA, tB; // parameters on line segment segmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, translation, directionA, capsuleLengthA, directionB, capsuleLengthB); btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; if (distance > distanceThreshold) return distance; btScalar lenSqr = ptsVector.length2(); if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON)) { //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA' btVector3 q; btPlaneSpace1(directionA, normalOnB, q); } else { // compute the contact normal normalOnB = ptsVector * -btRecipSqrt(lenSqr); } pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB; return distance; }
void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { (void)dispatchInfo; if (!m_manifoldPtr) return; btCollisionObject* convexObj = m_isSwapped? body1 : body0; btCollisionObject* planeObj = m_isSwapped? body0: body1; btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); const btVector3& planeNormal = planeShape->getPlaneNormal(); //const btScalar& planeConstant = planeShape->getPlaneConstant(); //first perform a collision query with the non-perturbated collision objects { btQuaternion rotq(0,0,0,1); collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); } if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) { btVector3 v0,v1; btPlaneSpace1(planeNormal,v0,v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radius = convexShape->getAngularMotionDisc(); perturbeAngle = gContactBreakingThreshold / radius; if ( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btQuaternion perturbeRot(v0,perturbeAngle); for (int i=0;i<m_numPerturbationIterations;i++) { btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(planeNormal,iterationAngle); collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut); } } if (m_ownManifold) { if (m_manifoldPtr->getNumContacts()) { resultOut->refreshContactPoints(); } } }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), #ifdef _BT_USE_CENTER_LIMIT_ m_limit(), #endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0), m_normalCFM(0), m_normalERP(0), m_stopCFM(0), m_stopERP(0) { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); #ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; #endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); }
void btConeTwistConstraint::buildJacobian() { if (m_useSolveConstraintObsolete) { m_appliedImpulse = btScalar(0.); m_accTwistLimitImpulse = btScalar(0.); m_accSwingLimitImpulse = btScalar(0.); m_accMotorImpulse = btVector3(0.,0.,0.); if (!m_angularOnly) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btVector3 relPos = pivotBInW - pivotAInW; btVector3 normal[3]; if (relPos.length2() > SIMD_EPSILON) { normal[0] = relPos.normalized(); } else { normal[0].setValue(btScalar(1.0),0,0); } btPlaneSpace1(normal[0], normal[1], normal[2]); for (int i=0;i<3;i++) { new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(), normal[i], m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); } } calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); } }
void ConstraintPhysicsSetup::stepSimulation(float deltaTime) { val = spDoorHinge->getAccumulatedHingeAngle() * SIMD_DEGS_PER_RAD; if (m_dynamicsWorld) { spDoorHinge->enableAngularMotor(true, targetVel, maxImpulse); m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.); btHingeConstraint* hinge = spDoorHinge; if (hinge) { const btRigidBody& bodyA = hinge->getRigidBodyA(); const btRigidBody& bodyB = hinge->getRigidBodyB(); btTransform trA = bodyA.getWorldTransform(); btVector3 angVelA = bodyA.getAngularVelocity(); btVector3 angVelB = bodyB.getAngularVelocity(); { btVector3 ax1 = trA.getBasis() * hinge->getFrameOffsetA().getBasis().getColumn(2); btScalar vel = angVelA.dot(ax1); vel -= angVelB.dot(ax1); printf("hinge velocity (q) = %f\n", vel); actualHingeVelocity = vel; } btVector3 ortho0, ortho1; btPlaneSpace1(btAxisA, ortho0, ortho1); { btScalar vel2 = angVelA.dot(ortho0); vel2 -= angVelB.dot(ortho0); printf("hinge orthogonal1 velocity (q) = %f\n", vel2); } { btScalar vel0 = angVelA.dot(ortho1); vel0 -= angVelB.dot(ortho1); printf("hinge orthogonal0 velocity (q) = %f\n", vel0); } } } }
void terrain::drawPlane(float x, float y) { const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(m_planeBody->getCollisionShape()); btVector3 normal = staticPlaneShape->getPlaneNormal(); //btVector3 planeOrigin = normal * constant; btVector3 planeOrigin = m_planeBody->getCenterOfMassPosition(); btVector3 vec0,vec1; btPlaneSpace1(normal,vec0,vec1); btVector3 pt0 = planeOrigin + (vec0*y + vec1*x); btVector3 pt1 = planeOrigin - (vec0*y + vec1*x); btVector3 pt2 = planeOrigin + (vec0*y - vec1*x); btVector3 pt3 = planeOrigin - (vec0*y - vec1*x); glNormal3f(normal.x(),normal.y(),normal.z()); glBegin(GL_QUADS); glVertex3f(pt0.getX(),pt0.getY(),pt0.getZ()); glVertex3f(pt3.getX(),pt3.getY(),pt3.getZ()); glVertex3f(pt1.getX(),pt1.getY(),pt1.getZ()); glVertex3f(pt2.getX(),pt2.getY(),pt2.getZ()); glEnd(); }
void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { btAssert(m_manifoldPtr); //order in manifold needs to match if (depth > m_manifoldPtr->getContactBreakingThreshold()) // if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); btVector3 pointA = pointInWorld + normalOnBInWorld * depth; btVector3 localA; btVector3 localB; if (isSwapped) { localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); } else { localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); } btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; int insertIndex = m_manifoldPtr->getCacheEntry(newPt); newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2); //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; newPt.m_index0 = m_index1; newPt.m_index1 = m_index0; } else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; newPt.m_index0 = m_index0; newPt.m_index1 = m_index1; } //printf("depth=%f\n", depth); ///@todo, check this for any side effects if (insertIndex >= 0) { //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); m_manifoldPtr->replaceContactPoint(newPt, insertIndex); } else { insertIndex = m_manifoldPtr->addManifoldPoint(newPt); } //User can override friction and/or restitution // DrChat: Removed for multithreading version /* if (gContactAddedCallback && //and if either of the two bodies requires custom material ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) { //experimental feature info, for per-triangle material etc. const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1); } */ }
void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax) { if (shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { btVector3 org(m[12], m[13], m[14]); btVector3 dx(m[0], m[1], m[2]); btVector3 dy(m[4], m[5], m[6]); // btVector3 dz(m[8], m[9], m[10]); const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); btVector3 halfExtent = boxShape->getHalfExtentsWithMargin(); dx *= halfExtent[0]; dy *= halfExtent[1]; // dz *= halfExtent[2]; glColor3f(1,1,1); glDisable(GL_LIGHTING); glLineWidth(2); glBegin(GL_LINE_LOOP); glDrawVector(org - dx - dy); glDrawVector(org - dx + dy); glDrawVector(org + dx + dy); glDrawVector(org + dx - dy); glEnd(); return; } else if((shape->getShapeType() == BOX_SHAPE_PROXYTYPE) && (debugMode & btIDebugDraw::DBG_FastWireframe)) { btVector3 org(m[12], m[13], m[14]); btVector3 dx(m[0], m[1], m[2]); btVector3 dy(m[4], m[5], m[6]); btVector3 dz(m[8], m[9], m[10]); const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); btVector3 halfExtent = boxShape->getHalfExtentsWithMargin(); dx *= halfExtent[0]; dy *= halfExtent[1]; dz *= halfExtent[2]; glBegin(GL_LINE_LOOP); glDrawVector(org - dx - dy - dz); glDrawVector(org + dx - dy - dz); glDrawVector(org + dx + dy - dz); glDrawVector(org - dx + dy - dz); glDrawVector(org - dx + dy + dz); glDrawVector(org + dx + dy + dz); glDrawVector(org + dx - dy + dz); glDrawVector(org - dx - dy + dz); glEnd(); glBegin(GL_LINES); glDrawVector(org + dx - dy - dz); glDrawVector(org + dx - dy + dz); glDrawVector(org + dx + dy - dz); glDrawVector(org + dx + dy + dz); glDrawVector(org - dx - dy - dz); glDrawVector(org - dx + dy - dz); glDrawVector(org - dx - dy + dz); glDrawVector(org - dx + dy + dz); glEnd(); return; } glPushMatrix(); btglMultMatrix(m); if (shape->getShapeType() == UNIFORM_SCALING_SHAPE_PROXYTYPE) { const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape); const btConvexShape* convexShape = scalingShape->getChildShape(); float scalingFactor = (float)scalingShape->getUniformScalingFactor(); { btScalar tmpScaling[4][4]={{scalingFactor,0,0,0}, {0,scalingFactor,0,0}, {0,0,scalingFactor,0}, {0,0,0,1}}; drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode,worldBoundsMin,worldBoundsMax); } glPopMatrix(); return; } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) { const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) { btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* colShape = compoundShape->getChildShape(i); ATTRIBUTE_ALIGNED16(btScalar) childMat[16]; childTrans.getOpenGLMatrix(childMat); drawOpenGL(childMat,colShape,color,debugMode,worldBoundsMin,worldBoundsMax); } } else { if(m_textureenabled&&(!m_textureinitialized)) { GLubyte* image=new GLubyte[256*256*4]; for(int y=0;y<256;++y) { const int t=y>>4; GLubyte* pi=image+y*256*3; for(int x=0;x<256;++x) { const int s=x>>4; const GLubyte b=180; GLubyte c=b+((s+(t&1))&1)*(255-b); pi[0]=pi[1]=pi[2]=pi[3]=c;pi+=3; } } glGenTextures(1,(GLuint*)&m_texturehandle); glBindTexture(GL_TEXTURE_2D,m_texturehandle); glGenTextures(1,(GLuint*)&m_texturehandle); glBindTexture(GL_TEXTURE_2D,m_texturehandle); glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR); glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR); glTexImage2D(GL_TEXTURE_2D, 0, 3, 256 , 256 , 0, GL_RGB, GL_UNSIGNED_BYTE, image); //glGenerateMipmap(GL_TEXTURE_2D); delete[] image; } glMatrixMode(GL_TEXTURE); glLoadIdentity(); glScalef(0.025f,0.025f,0.025f); glMatrixMode(GL_MODELVIEW); static const GLfloat planex[]={1,0,0,0}; // static const GLfloat planey[]={0,1,0,0}; static const GLfloat planez[]={0,0,1,0}; glTexGenfv(GL_S,GL_OBJECT_PLANE,planex); glTexGenfv(GL_T,GL_OBJECT_PLANE,planez); glTexGeni(GL_S,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR); glTexGeni(GL_T,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR); glEnable(GL_TEXTURE_GEN_S); glEnable(GL_TEXTURE_GEN_T); glEnable(GL_TEXTURE_GEN_R); m_textureinitialized=true; //drawCoordSystem(); //glPushMatrix(); glEnable(GL_COLOR_MATERIAL); if(m_textureenabled) { glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D,m_texturehandle); } else { glDisable(GL_TEXTURE_2D); } glColor3f(color.x(),color.y(), color.z()); //bool useWireframeFallback = true; if (!(debugMode & btIDebugDraw::DBG_DrawWireframe)) { ///you can comment out any of the specific cases, and use the default ///the benefit of 'default' is that it approximates the actual collision shape including collision margin //int shapetype=m_textureenabled?MAX_BROADPHASE_COLLISION_TYPES:shape->getShapeType(); int shapetype=shape->getShapeType(); switch (shapetype) { case SPHERE_SHAPE_PROXYTYPE: { const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin drawSphere(radius,10,10); //useWireframeFallback = false; break; } case BOX_SHAPE_PROXYTYPE: { const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); btVector3 halfExtent = boxShape->getHalfExtentsWithMargin(); static int indices[36] = { 0,1,2, 3,2,1, 4,0,6, 6,0,2, 5,1,4, 4,1,0, 7,3,1, 7,1,5, 5,4,7, 7,4,6, 7,2,3, 7,6,2}; btVector3 vertices[8]={ btVector3(halfExtent[0],halfExtent[1],halfExtent[2]), btVector3(-halfExtent[0],halfExtent[1],halfExtent[2]), btVector3(halfExtent[0],-halfExtent[1],halfExtent[2]), btVector3(-halfExtent[0],-halfExtent[1],halfExtent[2]), btVector3(halfExtent[0],halfExtent[1],-halfExtent[2]), btVector3(-halfExtent[0],halfExtent[1],-halfExtent[2]), btVector3(halfExtent[0],-halfExtent[1],-halfExtent[2]), btVector3(-halfExtent[0],-halfExtent[1],-halfExtent[2])}; #if 1 glBegin (GL_TRIANGLES); int si=36; for (int i=0;i<si;i+=3) { const btVector3& v1 = vertices[indices[i]];; const btVector3& v2 = vertices[indices[i+1]]; const btVector3& v3 = vertices[indices[i+2]]; btVector3 normal = (v3-v1).cross(v2-v1); normal.normalize (); glNormal3f(normal.getX(),normal.getY(),normal.getZ()); glVertex3f (v1.x(), v1.y(), v1.z()); glVertex3f (v2.x(), v2.y(), v2.z()); glVertex3f (v3.x(), v3.y(), v3.z()); } glEnd(); #endif //useWireframeFallback = false; break; } #if 0 case CONE_SHAPE_PROXYTYPE: { const btConeShape* coneShape = static_cast<const btConeShape*>(shape); int upIndex = coneShape->getConeUpIndex(); float radius = coneShape->getRadius();//+coneShape->getMargin(); float height = coneShape->getHeight();//+coneShape->getMargin(); switch (upIndex) { case 0: glRotatef(90.0, 0.0, 1.0, 0.0); break; case 1: glRotatef(-90.0, 1.0, 0.0, 0.0); break; case 2: break; default: { } }; glTranslatef(0.0, 0.0, -0.5*height); glutSolidCone(radius,height,10,10); //useWireframeFallback = false; break; } #endif case STATIC_PLANE_PROXYTYPE: { const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); btScalar planeConst = staticPlaneShape->getPlaneConstant(); const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); btVector3 planeOrigin = planeNormal * planeConst; btVector3 vec0,vec1; btPlaneSpace1(planeNormal,vec0,vec1); btScalar vecLen = 100.f; btVector3 pt0 = planeOrigin + vec0*vecLen; btVector3 pt1 = planeOrigin - vec0*vecLen; btVector3 pt2 = planeOrigin + vec1*vecLen; btVector3 pt3 = planeOrigin - vec1*vecLen; glBegin(GL_LINES); glVertex3f(pt0.getX(),pt0.getY(),pt0.getZ()); glVertex3f(pt1.getX(),pt1.getY(),pt1.getZ()); glVertex3f(pt2.getX(),pt2.getY(),pt2.getZ()); glVertex3f(pt3.getX(),pt3.getY(),pt3.getZ()); glEnd(); break; } case MULTI_SPHERE_SHAPE_PROXYTYPE: { const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); btTransform childTransform; childTransform.setIdentity(); for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) { btSphereShape sc(multiSphereShape->getSphereRadius(i)); childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); ATTRIBUTE_ALIGNED16(btScalar) childMat[16]; childTransform.getOpenGLMatrix(childMat); drawOpenGL(childMat,&sc,color,debugMode,worldBoundsMin,worldBoundsMax); } break; } default: { if (shape->isConvex()) { const btConvexPolyhedron* poly = shape->isPolyhedral() ? ((btPolyhedralConvexShape*) shape)->getConvexPolyhedron() : 0; if (poly) { int i; glBegin (GL_TRIANGLES); for (i=0;i<poly->m_faces.size();i++) { btVector3 centroid(0,0,0); int numVerts = poly->m_faces[i].m_indices.size(); if (numVerts>2) { btVector3 v1 = poly->m_vertices[poly->m_faces[i].m_indices[0]]; for (int v=0;v<poly->m_faces[i].m_indices.size()-2;v++) { btVector3 v2 = poly->m_vertices[poly->m_faces[i].m_indices[v+1]]; btVector3 v3 = poly->m_vertices[poly->m_faces[i].m_indices[v+2]]; btVector3 normal = (v3-v1).cross(v2-v1); normal.normalize (); glNormal3f(normal.getX(),normal.getY(),normal.getZ()); glVertex3f (v1.x(), v1.y(), v1.z()); glVertex3f (v2.x(), v2.y(), v2.z()); glVertex3f (v3.x(), v3.y(), v3.z()); } } } glEnd (); } else { ShapeCache* sc=cache((btConvexShape*)shape); //glutSolidCube(1.0); btShapeHull* hull = &sc->m_shapehull/*(btShapeHull*)shape->getUserPointer()*/; if (hull->numTriangles () > 0) { int index = 0; const unsigned int* idx = hull->getIndexPointer(); const btVector3* vtx = hull->getVertexPointer(); glBegin (GL_TRIANGLES); for (int i = 0; i < hull->numTriangles (); i++) { int i1 = index++; int i2 = index++; int i3 = index++; btAssert(i1 < hull->numIndices () && i2 < hull->numIndices () && i3 < hull->numIndices ()); int index1 = idx[i1]; int index2 = idx[i2]; int index3 = idx[i3]; btAssert(index1 < hull->numVertices () && index2 < hull->numVertices () && index3 < hull->numVertices ()); btVector3 v1 = vtx[index1]; btVector3 v2 = vtx[index2]; btVector3 v3 = vtx[index3]; btVector3 normal = (v3-v1).cross(v2-v1); normal.normalize (); glNormal3f(normal.getX(),normal.getY(),normal.getZ()); glVertex3f (v1.x(), v1.y(), v1.z()); glVertex3f (v2.x(), v2.y(), v2.z()); glVertex3f (v3.x(), v3.y(), v3.z()); } glEnd (); } } } } } } glNormal3f(0,1,0); /// for polyhedral shapes if (debugMode==btIDebugDraw::DBG_DrawFeaturesText && (shape->isPolyhedral())) { btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; { glColor3f(1.f, 1.f, 1.f); int i; for (i=0;i<polyshape->getNumVertices();i++) { btVector3 vtx; polyshape->getVertex(i,vtx); char buf[12]; sprintf(buf," %d",i); //btDrawString(BMF_GetFont(BMF_kHelvetica10),buf); } for (i=0;i<polyshape->getNumPlanes();i++) { btVector3 normal; btVector3 vtx; polyshape->getPlane(normal,vtx,i); //btScalar d = vtx.dot(normal); //char buf[12]; //sprintf(buf," plane %d",i); //btDrawString(BMF_GetFont(BMF_kHelvetica10),buf); } } } }
void btHingeConstraint::buildJacobian() { if (m_useSolveConstraintObsolete) { m_appliedImpulse = btScalar(0.); m_accMotorImpulse = btScalar(0.); if (!m_angularOnly) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btVector3 relPos = pivotBInW - pivotAInW; btVector3 normal[3]; if (relPos.length2() > SIMD_EPSILON) { normal[0] = relPos.normalized(); } else { normal[0].setValue(btScalar(1.0),0,0); } btPlaneSpace1(normal[0], normal[1], normal[2]); for (int i=0;i<3;i++) { new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(), normal[i], m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); } } //calculate two perpendicular jointAxis, orthogonal to hingeAxis //these two jointAxis require equal angular velocities for both bodies //this is unused for now, it's a todo btVector3 jointAxis0local; btVector3 jointAxis1local; btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); new (&m_jacAng[0]) btJacobianEntry(jointAxis0, m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getInvInertiaDiagLocal(), m_rbB.getInvInertiaDiagLocal()); new (&m_jacAng[1]) btJacobianEntry(jointAxis1, m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getInvInertiaDiagLocal(), m_rbB.getInvInertiaDiagLocal()); new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getInvInertiaDiagLocal(), m_rbB.getInvInertiaDiagLocal()); // clear accumulator m_accLimitImpulse = btScalar(0.); // test angular limit testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); //Compute K = J*W*J' for hinge axis btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + getRigidBodyB().computeAngularImpulseDenominator(axisA)); } }
static __inline void solveFriction(Constraint4& cs, const float4& posA, float4& linVelA, float4& angVelA, float invMassA, const Matrix3x3& invInertiaA, const float4& posB, float4& linVelB, float4& angVelB, float invMassB, const Matrix3x3& invInertiaB, float maxRambdaDt[4], float minRambdaDt[4]) { if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; const float4& center = cs.m_center; float4 n = -cs.m_linear; float4 tangent[2]; #if 1 btPlaneSpace1 (&n, &tangent[0],&tangent[1]); #else float4 r = cs.m_worldPos[0]-center; tangent[0] = cross3( n, r ); tangent[1] = cross3( tangent[0], n ); tangent[0] = normalize3( tangent[0] ); tangent[1] = normalize3( tangent[1] ); #endif float4 angular0, angular1, linear; float4 r0 = center - posA; float4 r1 = center - posB; for(int i=0; i<2; i++) { setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 ); float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, linVelA, angVelA, linVelB, angVelB ); rambdaDt *= cs.m_fJacCoeffInv[i]; { float prevSum = cs.m_fAppliedRambdaDt[i]; float updated = prevSum; updated += rambdaDt; updated = max2( updated, minRambdaDt[i] ); updated = min2( updated, maxRambdaDt[i] ); rambdaDt = updated - prevSum; cs.m_fAppliedRambdaDt[i] = updated; } float4 linImp0 = invMassA*linear*rambdaDt; float4 linImp1 = invMassB*(-linear)*rambdaDt; float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; #ifdef _WIN32 btAssert(_finite(linImp0.x)); btAssert(_finite(linImp1.x)); #endif linVelA += linImp0; angVelA += angImp0; linVelB += linImp1; angVelB += angImp1; } { // angular damping for point constraint float4 ab = normalize3( posB - posA ); float4 ac = normalize3( center - posA ); if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) { float angNA = dot3F4( n, angVelA ); float angNB = dot3F4( n, angVelB ); angVelA -= (angNA*0.1f)*n; angVelB -= (angNB*0.1f)*n; } } }
void kSetupContact(btParallelConstraintSolver* pSolver, btParallelConstraintSolverSetupTaskParams* pParams, btContactSolverInfo* pInfoGlobal, int threadId) { int numConstraints = pParams[threadId].m_numContactConstraints; unsigned long int timeStamp; int startIndex = pParams[threadId].m_startIndex; btContactSolverInfo& infoGlobal = *pInfoGlobal; for(int i = 0; i < numConstraints; i++) { timeStamp = sClock.getTimeMicroseconds(); btSolverConstraint& solverConstraint = pSolver->m_tmpSolverContactConstraintPool[startIndex + i]; solverConstraint.m_numConsecutiveRowsPerKernel = timeStamp; btCollisionObject* colObj0 = (btCollisionObject*)solverConstraint.m_solverBodyA; btCollisionObject* colObj1 = (btCollisionObject*)solverConstraint.m_solverBodyB; btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); btManifoldPoint& cp = *((btManifoldPoint*)(solverConstraint.m_originalContactPoint)); btVector3 rel_pos1; btVector3 rel_pos2; btScalar relaxation; btScalar rel_vel; btVector3 vel; pSolver->setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); int currFrictIndex = solverConstraint.m_frictionIndex; if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) { cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if(!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); cp.m_lateralFrictionDir2.normalize();//?? applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex]; currFrictIndex++; pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex]; currFrictIndex++; pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); cp.m_lateralFrictionInitialized = true; } else { //re-calculate friction direction every frame, todo: check if this is really needed btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex]; currFrictIndex++; pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex]; currFrictIndex++; pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); cp.m_lateralFrictionInitialized = true; } } else { btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex]; currFrictIndex++; pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex]; currFrictIndex++; pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); } } pSolver->setFrictionConstraintImpulse( solverConstraint, solverBodyA, solverBodyB, cp, infoGlobal); } }
// // Convex-Convex collision algorithm // void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { if (!m_manifoldPtr) { //swapped? m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape()); btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape()); btVector3 normalOnB; btVector3 pointOnBWorld; #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { btCapsuleShape* capsuleA = (btCapsuleShape*) min0; btCapsuleShape* capsuleB = (btCapsuleShape*) min1; btVector3 localScalingA = capsuleA->getLocalScaling(); btVector3 localScalingB = capsuleB->getLocalScaling(); btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), body0->getWorldTransform(),body1->getWorldTransform(),threshold); if (dist<threshold) { btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); } resultOut->refreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) #endif //USE_SEPDISTANCE_UTIL2 { btGjkPairDetector::ClosestPointInput input; btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA(min0); gjkPairDetector.setMinkowskiB(min1); #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { if (dispatchInfo.m_convexMaxDistanceUseCPT) { input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); } else { input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); } input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #ifdef USE_SEPDISTANCE_UTIL2 btScalar sepDist = 0.f; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { int i; btVector3 v0,v1; btVector3 sepNormalWorldSpace; sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); btPlaneSpace1(sepNormalWorldSpace,v0,v1); bool perturbeA = true; const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radiusA = min0->getAngularMotionDisc(); btScalar radiusB = min1->getAngularMotionDisc(); if (radiusA < radiusB) { perturbeAngle = gContactBreakingThreshold /radiusA; perturbeA = true; } else { perturbeAngle = gContactBreakingThreshold / radiusB; perturbeA = false; } if ( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btTransform unPerturbedTransform; if (perturbeA) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for ( i=0;i<m_numPerturbationIterations;i++) { if (v0.length2()>SIMD_EPSILON) { btQuaternion perturbeRot(v0,perturbeAngle); btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(sepNormalWorldSpace,iterationAngle); if (perturbeA) { input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); input.m_transformB = body1->getWorldTransform(); #ifdef DEBUG_CONTACTS dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); #endif //DEBUG_CONTACTS } else { input.m_transformA = body0->getWorldTransform(); input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); #ifdef DEBUG_CONTACTS dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); #endif } btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); } } } #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 } if (m_ownManifold) { resultOut->refreshContactPoints(); } }
void btConeTwistConstraint::buildJacobian() { m_appliedImpulse = btScalar(0.); //set bias, sign, clear accumulator m_swingCorrection = btScalar(0.); m_twistLimitSign = btScalar(0.); m_solveTwistLimit = false; m_solveSwingLimit = false; m_accTwistLimitImpulse = btScalar(0.); m_accSwingLimitImpulse = btScalar(0.); if (!m_angularOnly) { btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); btVector3 relPos = pivotBInW - pivotAInW; btVector3 normal[3]; if (relPos.length2() > SIMD_EPSILON) { normal[0] = relPos.normalized(); } else { normal[0].setValue(btScalar(1.0),0,0); } btPlaneSpace1(normal[0], normal[1], normal[2]); for (int i=0;i<3;i++) { new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(), normal[i], m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); } } btVector3 b1Axis1,b1Axis2,b1Axis3; btVector3 b2Axis1,b2Axis2; b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); btScalar swing1=btScalar(0.),swing2 = btScalar(0.); // Get Frame into world space if (m_swingSpan1 >= btScalar(0.05f)) { b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); } if (m_swingSpan2 >= btScalar(0.05f)) { b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); } btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { m_swingCorrection = EllipseAngle-1.0f; m_solveSwingLimit = true; // Calculate necessary axis & factors m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); m_swingAxis.normalize(); btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); } // Twist limits if (m_twistSpan >= btScalar(0.)) { btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); if (twist <= -m_twistSpan*lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_twistAxis *= -1.0f; m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); } else if (twist > m_twistSpan*lockedFreeFactor) { m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); } } }
void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); btMultiBody* mbA = fcA? fcA->m_multiBody : 0; btMultiBody* mbB = fcB? fcB->m_multiBody : 0; btCollisionObject* colObj0=0,*colObj1=0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; ///avoid collision response between two static objects // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) // return; for (int j=0;j<manifold->getNumContacts();j++) { btManifoldPoint& cp = manifold->getContactPoint(j); if (cp.getDistance() <= manifold->getContactProcessingThreshold()) { btScalar relaxation; int frictionIndex = m_multiBodyNormalContactConstraints.size(); btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); // btRigidBody* rb0 = btRigidBody::upcast(colObj0); // btRigidBody* rb1 = btRigidBody::upcast(colObj1); solverConstraint.m_orgConstraint = 0; solverConstraint.m_orgDofIndex = -1; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_multiBodyA = mbA; if (mbA) solverConstraint.m_linkA = fcA->m_link; solverConstraint.m_multiBodyB = mbB; if (mbB) solverConstraint.m_linkB = fcB->m_link; solverConstraint.m_originalContactPoint = &cp; bool isFriction = false; setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); /////setup the friction constraints #define ENABLE_FRICTION #ifdef ENABLE_FRICTION solverConstraint.m_frictionIndex = frictionIndex; #if ROLLING_FRICTION int rollingFriction=1; btVector3 angVelA(0,0,0),angVelB(0,0,0); if (rb0) angVelA = rb0->getAngularVelocity(); if (rb1) angVelB = rb1->getAngularVelocity(); btVector3 relAngVel = angVelB-angVelA; if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) { //only a single rollingFriction per manifold rollingFriction--; if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) { relAngVel.normalize(); applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); if (relAngVel.length()>0.001) addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } else { addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); btVector3 axis0,axis1; btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); if (axis0.length()>0.001) addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if (axis1.length()>0.001) addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } } #endif //ROLLING_FRICTION ///Bullet has several options to set the friction directions ///By default, each contact has only a single friction direction that is recomputed automatically very frame ///based on the relative linear velocity. ///If the relative velocity it zero, it will automatically compute a friction direction. ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. /// ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. /// ///The user can manually override the friction directions for certain contacts using a contact callback, ///and set the cp.m_lateralFrictionInitialized to true ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) {/* cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); cp.m_lateralFrictionDir2.normalize();//?? applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } else */ { btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { cp.m_lateralFrictionInitialized = true; } } } else { addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); //todo: solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; } #endif //ENABLE_FRICTION } } }
// // Convex-Convex collision algorithm // void btConvexConvexAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) { //swapped? m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape()); const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape()); btVector3 normalOnB; btVector3 pointOnBWorld; #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { btCapsuleShape* capsuleA = (btCapsuleShape*) min0; btCapsuleShape* capsuleB = (btCapsuleShape*) min1; // btVector3 localScalingA = capsuleA->getLocalScaling(); // btVector3 localScalingB = capsuleB->getLocalScaling(); btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(), capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); if (dist<threshold) { btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); } resultOut->refreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) #endif //USE_SEPDISTANCE_UTIL2 btGjkPairDetector::ClosestPointInput input; btGjkPairDetector gjkPairDetector(min0, min1, &m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA(min0); gjkPairDetector.setMinkowskiB(min1); #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { //if (dispatchInfo.m_convexMaxDistanceUseCPT) //{ // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); //} else //{ input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); // } input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); #ifdef USE_SEPDISTANCE_UTIL2 btScalar sepDist = 0.f; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 if (min0->isPolyhedral() && min1->isPolyhedral()) { struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result { virtual void setShapeIdentifiersA(int partId0, int index0){} virtual void setShapeIdentifiersB(int partId1, int index1){} virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { } }; struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result { btDiscreteCollisionDetectorInterface::Result* m_originalResult; btVector3 m_reportedNormalOnWorld; btScalar m_marginOnA; btScalar m_marginOnB; btScalar m_reportedDistance; bool m_foundResult; btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) :m_originalResult(result), m_marginOnA(marginOnA), m_marginOnB(marginOnB), m_foundResult(false) { } virtual void setShapeIdentifiersA(int partId0, int index0){} virtual void setShapeIdentifiersB(int partId1, int index1){} virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg) { m_reportedDistance = depthOrg; m_reportedNormalOnWorld = normalOnBInWorld; btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB; m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB); if (m_reportedDistance<0.f) { m_foundResult = true; } m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance); } }; btDummyResult dummy; ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin(); btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin(); btWithoutMarginResult withoutMargin(resultOut, min0Margin, min1Margin); btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) { btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); btScalar minDist = -1e30f; btVector3 sepNormalWorldSpace; bool foundSepAxis = true; if (dispatchInfo.m_enableSatConvex) { foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), sepNormalWorldSpace,*resultOut); } else { #ifdef ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw); //gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw); #endif //ZERO_MARGIN //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); //if (l2>SIMD_EPSILON) { sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); #ifdef ZERO_MARGIN foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; #else foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin()); #endif } } if (foundSepAxis) { //printf("sepNormalWorldSpace=%f, %f, %f\n", sepNormalWorldSpace.getX(), sepNormalWorldSpace.getY(), sepNormalWorldSpace.getZ()); btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut); } if (m_ownManifold) { resultOut->refreshContactPoints(); } return; } else { //we can also deal with convex versus triangle (without connectivity data) if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) { btVertexArray vertices; btTriangleShape* tri = (btTriangleShape*)polyhedronB; vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); //tri->initializePolyhedralFeatures(); btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); btVector3 sepNormalWorldSpace; btScalar minDist =-1e30f; btScalar maxDist = threshold; bool foundSepAxis = false; if (0) { polyhedronB->initializePolyhedralFeatures(); foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), sepNormalWorldSpace,*resultOut); // printf("sepNormalWorldSpace=%f, %f, %f\n", sepNormalWorldSpace.getX(), sepNormalWorldSpace.getY(), sepNormalWorldSpace.getZ()); } else { #ifdef ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if (l2>SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); //minDist = gjkPairDetector.getCachedSeparatingDistance(); //maxDist = threshold; minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); foundSepAxis = true; } } if (foundSepAxis) { btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); } if (m_ownManifold) { resultOut->refreshContactPoints(); } return; } } } gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { int i; btVector3 v0, v1; btVector3 sepNormalWorldSpace; btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if (l2 > SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); btPlaneSpace1(sepNormalWorldSpace, v0, v1); bool perturbeA = true; const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radiusA = min0->getAngularMotionDisc(); btScalar radiusB = min1->getAngularMotionDisc(); if (radiusA < radiusB) { perturbeAngle = gContactBreakingThreshold /radiusA; perturbeA = true; } else { perturbeAngle = gContactBreakingThreshold / radiusB; perturbeA = false; } if ( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btTransform unPerturbedTransform; if (perturbeA) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for ( i=0;i<m_numPerturbationIterations;i++) { if (v0.length2()>SIMD_EPSILON) { btQuaternion perturbeRot(v0, perturbeAngle); btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(sepNormalWorldSpace, iterationAngle); if (perturbeA) { input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis()); input.m_transformB = body1Wrap->getWorldTransform(); #ifdef DEBUG_CONTACTS dispatchInfo.m_debugDraw->drawTransform(input.m_transformA, 5.0); #endif //DEBUG_CONTACTS } else { input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis()); #ifdef DEBUG_CONTACTS dispatchInfo.m_debugDraw->drawTransform(input.m_transformB, 5.0); #endif } btPerturbedContactResult perturbedResultOut(resultOut, input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(input, perturbedResultOut, dispatchInfo.m_debugDraw); } } } } #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0->getWorldTransform(), body1->getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 if (m_ownManifold) { resultOut->refreshContactPoints(); } }
btTypedConstraint* gkDynamicsWorld::createConstraint(btRigidBody* rbA, btRigidBody*rbB, const gkPhysicsConstraintProperties& props) { btVector3 pivotInA(gkMathUtils::get(props.m_pivot)); btVector3 pivotInB(0,0,0); pivotInB = rbB ? rbB->getCenterOfMassTransform().inverse()(rbA->getCenterOfMassTransform()(pivotInA)) : rbA->getCenterOfMassTransform() * pivotInA; //localConstraintFrameBasis btMatrix3x3 localCFrame; localCFrame.setEulerZYX(props.m_axis.x, props.m_axis.y, props.m_axis.z); btVector3 axisInA = localCFrame.getColumn(0); btVector3 axis1 = localCFrame.getColumn(1); btVector3 axis2 = localCFrame.getColumn(2); bool angularOnly = false; btTypedConstraint* constraint = 0; if (props.m_type == GK_BALL_CONSTRAINT) { btPoint2PointConstraint* p2p = 0; if (rbB) p2p = new btPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB); else p2p = new btPoint2PointConstraint(*rbA,pivotInA); constraint = p2p; } else if (props.m_type == GK_HINGE_CONSTRAINT) { btHingeConstraint* hinge = 0; if (rbB) { btVector3 axisInB = rbB->getCenterOfMassTransform().getBasis().inverse() * (rbA->getCenterOfMassTransform().getBasis() * axisInA); hinge = new btHingeConstraint(*rbA, *rbB, pivotInA, pivotInB, axisInA, axisInB); } else { hinge = new btHingeConstraint(*rbA, pivotInA, axisInA); } hinge->setAngularOnly(angularOnly); constraint = hinge; } else if (props.m_type == GK_D6_CONSTRAINT) { btTransform frameInA; btTransform frameInB; if (axis1.length() == 0.0) { btPlaneSpace1(axisInA, axis1, axis2); } frameInA.getBasis().setValue(axisInA.x(), axis1.x(), axis2.x(), axisInA.y(), axis1.y(), axis2.y(), axisInA.z(), axis1.z(), axis2.z()); frameInA.setOrigin( pivotInA ); btTransform inv = rbB ? rbB->getCenterOfMassTransform().inverse() : btTransform::getIdentity(); btTransform globalFrameA = rbA->getCenterOfMassTransform() * frameInA; frameInB = inv * globalFrameA; bool useReferenceFrameA = true; if (!rbB) rbB = getFixedBody(); btGeneric6DofSpringConstraint* genericConstraint = new btGeneric6DofSpringConstraint(*rbA, *rbB, frameInA, frameInB, useReferenceFrameA); //if it is a generic 6DOF constraint, set all the limits accordingly int dof, dofbit=1; for (dof=0;dof<6;dof++) { if (props.m_flag & dofbit) { ///this access is a bloated, will probably make special cases for common arrays btScalar minLimit = props.m_minLimit[dof]; btScalar maxLimit = props.m_maxLimit[dof]; genericConstraint->setLimit(dof, minLimit, maxLimit); } else { //minLimit > maxLimit means free(disabled limit) for this degree of freedom genericConstraint->setLimit(dof, 1, -1); } dofbit<<=1; } constraint = genericConstraint; } else if (props.m_type == GK_CONETWIST_CONSTRAINT) { //TODO: implement conetwist constraint } return constraint; }
void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) { const btTransform& trA = getCalculatedTransformA(); const btTransform& trB = getCalculatedTransformB(); btAssert(!m_useSolveConstraintObsolete); int i, s = info->rowskip; btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); // difference between frames in WCS btVector3 ofs = trB.getOrigin() - trA.getOrigin(); // now get weight factors depending on masses btScalar miA = rbAinvMass; btScalar miB = rbBinvMass; bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); btScalar miS = miA + miB; btScalar factA, factB; if(miS > btScalar(0.f)) { factA = miB / miS; } else { factA = btScalar(0.5f); } factB = btScalar(1.0f) - factA; btVector3 ax1, p, q; btVector3 ax1A = trA.getBasis().getColumn(0); btVector3 ax1B = trB.getBasis().getColumn(0); if(m_useOffsetForConstraintFrame) { // get the desired direction of slider axis // as weighted sum of X-orthos of frameA and frameB in WCS ax1 = ax1A * factA + ax1B * factB; ax1.normalize(); // construct two orthos to slider axis btPlaneSpace1 (ax1, p, q); } else { // old way - use frameA ax1 = trA.getBasis().getColumn(0); // get 2 orthos to slider axis (Y, Z) p = trA.getBasis().getColumn(1); q = trA.getBasis().getColumn(2); } // make rotations around these orthos equal // the slider axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the slider axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the slider axis, and w1 and w2 // are the angular velocity vectors of the two bodies. info->m_J1angularAxis[0] = p[0]; info->m_J1angularAxis[1] = p[1]; info->m_J1angularAxis[2] = p[2]; info->m_J1angularAxis[s+0] = q[0]; info->m_J1angularAxis[s+1] = q[1]; info->m_J1angularAxis[s+2] = q[2]; info->m_J2angularAxis[0] = -p[0]; info->m_J2angularAxis[1] = -p[1]; info->m_J2angularAxis[2] = -p[2]; info->m_J2angularAxis[s+0] = -q[0]; info->m_J2angularAxis[s+1] = -q[1]; info->m_J2angularAxis[s+2] = -q[2]; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the slider back into alignment. // if ax1A,ax1B are the unit length slider axes as computed from bodyA and // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. // btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp; btScalar k = info->fps * currERP; btVector3 u = ax1A.cross(ax1B); info->m_constraintError[0] = k * u.dot(p); info->m_constraintError[s] = k * u.dot(q); if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) { info->cfm[0] = m_cfmOrthoAng; info->cfm[s] = m_cfmOrthoAng; } int nrow = 1; // last filled row int srow; btScalar limit_err; int limit; int powered; // next two rows. // we want: velA + wA x relA == velB + wB x relB ... but this would // result in three equations, so we project along two orthos to the slider axis btTransform bodyA_trans = transA; btTransform bodyB_trans = transB; nrow++; int s2 = nrow * s; nrow++; int s3 = nrow * s; btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0); if(m_useOffsetForConstraintFrame) { // get vector from bodyB to frameB in WCS relB = trB.getOrigin() - bodyB_trans.getOrigin(); // get its projection to slider axis btVector3 projB = ax1 * relB.dot(ax1); // get vector directed from bodyB to slider axis (and orthogonal to it) btVector3 orthoB = relB - projB; // same for bodyA relA = trA.getOrigin() - bodyA_trans.getOrigin(); btVector3 projA = ax1 * relA.dot(ax1); btVector3 orthoA = relA - projA; // get desired offset between frames A and B along slider axis btScalar sliderOffs = m_linPos - m_depth[0]; // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis btVector3 totalDist = projA + ax1 * sliderOffs - projB; // get offset vectors relA and relB relA = orthoA + totalDist * factA; relB = orthoB - totalDist * factB; // now choose average ortho to slider axis p = orthoB * factA + orthoA * factB; btScalar len2 = p.length2(); if(len2 > SIMD_EPSILON) { p /= btSqrt(len2); } else { p = trA.getBasis().getColumn(1); } // make one more ortho q = ax1.cross(p); // fill two rows tmpA = relA.cross(p); tmpB = relB.cross(p); for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; tmpA = relA.cross(q); tmpB = relB.cross(q); if(hasStaticBody && getSolveAngLimit()) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; } else { // old way - maybe incorrect if bodies are not on the slider axis // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); btVector3 tmp = c.cross(p); for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; tmp = c.cross(q); for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; } // compute two elements of right hand side // k = info->fps * info->erp * getSoftnessOrthoLin(); currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp; k = info->fps * currERP; btScalar rhs = k * p.dot(ofs); info->m_constraintError[s2] = rhs; rhs = k * q.dot(ofs); info->m_constraintError[s3] = rhs; if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) { info->cfm[s2] = m_cfmOrthoLin; info->cfm[s3] = m_cfmOrthoLin; } // check linear limits limit_err = btScalar(0.0); limit = 0; if(getSolveLinLimit()) { limit_err = getLinDepth() * signFact; limit = (limit_err > btScalar(0.0)) ? 2 : 1; } powered = 0; if(getPoweredLinMotor()) { powered = 1; } // if the slider has joint limits or motor, add in the extra row if (limit || powered) { nrow++; srow = nrow * info->rowskip; info->m_J1linearAxis[srow+0] = ax1[0]; info->m_J1linearAxis[srow+1] = ax1[1]; info->m_J1linearAxis[srow+2] = ax1[2]; info->m_J2linearAxis[srow+0] = -ax1[0]; info->m_J2linearAxis[srow+1] = -ax1[1]; info->m_J2linearAxis[srow+2] = -ax1[2]; // linear torque decoupling step: // // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies // do not create a torque couple. in other words, the points that the // constraint force is applied at must lie along the same ax1 axis. // a torque couple will result in limited slider-jointed free // bodies from gaining angular momentum. if(m_useOffsetForConstraintFrame) { // this is needed only when bodyA and bodyB are both dynamic. if(!hasStaticBody) { tmpA = relA.cross(ax1); tmpB = relB.cross(ax1); info->m_J1angularAxis[srow+0] = tmpA[0]; info->m_J1angularAxis[srow+1] = tmpA[1]; info->m_J1angularAxis[srow+2] = tmpA[2]; info->m_J2angularAxis[srow+0] = -tmpB[0]; info->m_J2angularAxis[srow+1] = -tmpB[1]; info->m_J2angularAxis[srow+2] = -tmpB[2]; } } else { // The old way. May be incorrect if bodies are not on the slider axis btVector3 ltd; // Linear Torque Decoupling vector (a torque) ltd = c.cross(ax1); info->m_J1angularAxis[srow+0] = factA*ltd[0]; info->m_J1angularAxis[srow+1] = factA*ltd[1]; info->m_J1angularAxis[srow+2] = factA*ltd[2]; info->m_J2angularAxis[srow+0] = factB*ltd[0]; info->m_J2angularAxis[srow+1] = factB*ltd[1]; info->m_J2angularAxis[srow+2] = factB*ltd[2]; } // right-hand part btScalar lostop = getLowerLinLimit(); btScalar histop = getUpperLinLimit(); if(limit && (lostop == histop)) { // the joint motor is ineffective powered = 0; } info->m_constraintError[srow] = 0.; info->m_lowerLimit[srow] = 0.; info->m_upperLimit[srow] = 0.; currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; if(powered) { if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) { info->cfm[srow] = m_cfmDirLin; } btScalar tag_vel = getTargetLinMotorVelocity(); btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; } if(limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) { info->cfm[srow] = m_cfmLimLin; } if(lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } else if(limit == 1) { // low limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } else { // high limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); if(bounce > btScalar(0.0)) { btScalar vel = linVelA.dot(ax1); vel -= linVelB.dot(ax1); vel *= signFact; // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if(limit == 1) { // low limit if(vel < 0) { btScalar newc = -bounce * vel; if (newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else { // high limit - all those computations are reversed if(vel > 0) { btScalar newc = -bounce * vel; if(newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } } info->m_constraintError[srow] *= getSoftnessLimLin(); } // if(limit) } // if linear limit // check angular limits limit_err = btScalar(0.0); limit = 0; if(getSolveAngLimit()) { limit_err = getAngDepth(); limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the slider has joint limits, add in the extra row powered = 0; if(getPoweredAngMotor()) { powered = 1; } if(limit || powered) { nrow++; srow = nrow * info->rowskip; info->m_J1angularAxis[srow+0] = ax1[0]; info->m_J1angularAxis[srow+1] = ax1[1]; info->m_J1angularAxis[srow+2] = ax1[2]; info->m_J2angularAxis[srow+0] = -ax1[0]; info->m_J2angularAxis[srow+1] = -ax1[1]; info->m_J2angularAxis[srow+2] = -ax1[2]; btScalar lostop = getLowerAngLimit(); btScalar histop = getUpperAngLimit(); if(limit && (lostop == histop)) { // the joint motor is ineffective powered = 0; } currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; if(powered) { if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) { info->cfm[srow] = m_cfmDirAng; } btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; } if(limit) { k = info->fps * currERP; info->m_constraintError[srow] += k * limit_err; if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) { info->cfm[srow] = m_cfmLimAng; } if(lostop == histop) { // limited low and high simultaneously info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = SIMD_INFINITY; } else if(limit == 1) { // low limit info->m_lowerLimit[srow] = 0; info->m_upperLimit[srow] = SIMD_INFINITY; } else { // high limit info->m_lowerLimit[srow] = -SIMD_INFINITY; info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); if(bounce > btScalar(0.0)) { btScalar vel = m_rbA.getAngularVelocity().dot(ax1); vel -= m_rbB.getAngularVelocity().dot(ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if(limit == 1) { // low limit if(vel < 0) { btScalar newc = -bounce * vel; if(newc > info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } else { // high limit - all those computations are reversed if(vel > 0) { btScalar newc = -bounce * vel; if(newc < info->m_constraintError[srow]) { info->m_constraintError[srow] = newc; } } } } info->m_constraintError[srow] *= getSoftnessLimAng(); } // if(limit) } // if angular limit or powered }
void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray<btVector3>& vertexPositions, btAlignedObjectArray<btVector3>& vertexNormals, btAlignedObjectArray<int>& indicesOut) { //todo: support all collision shape types switch (collisionShape->getShapeType()) { case SOFTBODY_SHAPE_PROXYTYPE: { //skip the soft body collision shape for now break; } case STATIC_PLANE_PROXYTYPE: { //draw a box, oriented along the plane normal const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(collisionShape); btScalar planeConst = staticPlaneShape->getPlaneConstant(); const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); btVector3 planeOrigin = planeNormal * planeConst; btVector3 vec0, vec1; btPlaneSpace1(planeNormal, vec0, vec1); btScalar vecLen = 100.f; btVector3 verts[4]; verts[0] = planeOrigin + vec0 * vecLen + vec1 * vecLen; verts[1] = planeOrigin - vec0 * vecLen + vec1 * vecLen; verts[2] = planeOrigin - vec0 * vecLen - vec1 * vecLen; verts[3] = planeOrigin + vec0 * vecLen - vec1 * vecLen; int startIndex = vertexPositions.size(); indicesOut.push_back(startIndex + 0); indicesOut.push_back(startIndex + 1); indicesOut.push_back(startIndex + 2); indicesOut.push_back(startIndex + 0); indicesOut.push_back(startIndex + 2); indicesOut.push_back(startIndex + 3); btVector3 triNormal = parentTransform.getBasis() * planeNormal; for (int i = 0; i < 4; i++) { btVector3 vtxPos; btVector3 pos = parentTransform * verts[i]; vertexPositions.push_back(pos); vertexNormals.push_back(triNormal); } break; } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)collisionShape; btVector3 trimeshScaling = trimesh->getLocalScaling(); btStridingMeshInterface* meshInterface = trimesh->getMeshInterface(); btAlignedObjectArray<btVector3> vertices; btAlignedObjectArray<int> indices; for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++) { const unsigned char* vertexbase = 0; int numverts = 0; PHY_ScalarType type = PHY_INTEGER; int stride = 0; const unsigned char* indexbase = 0; int indexstride = 0; int numfaces = 0; PHY_ScalarType indicestype = PHY_INTEGER; //PHY_ScalarType indexType=0; btVector3 triangleVerts[3]; meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId); btVector3 aabbMin, aabbMax; for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++) { unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride); for (int j = 2; j >= 0; j--) { int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); triangleVerts[j] = btVector3( graphicsbase[0] * trimeshScaling.getX(), graphicsbase[1] * trimeshScaling.getY(), graphicsbase[2] * trimeshScaling.getZ()); } else { double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * trimeshScaling.getX()), btScalar(graphicsbase[1] * trimeshScaling.getY()), btScalar(graphicsbase[2] * trimeshScaling.getZ())); } } indices.push_back(vertices.size()); vertices.push_back(triangleVerts[0]); indices.push_back(vertices.size()); vertices.push_back(triangleVerts[1]); indices.push_back(vertices.size()); vertices.push_back(triangleVerts[2]); btVector3 triNormal = (triangleVerts[1] - triangleVerts[0]).cross(triangleVerts[2] - triangleVerts[0]); btScalar dot = triNormal.dot(triNormal); //cull degenerate triangles if (dot >= SIMD_EPSILON * SIMD_EPSILON) { triNormal /= btSqrt(dot); for (int v = 0; v < 3; v++) { btVector3 pos = parentTransform * triangleVerts[v]; indicesOut.push_back(vertexPositions.size()); vertexPositions.push_back(pos); vertexNormals.push_back(triNormal); } } } } break; } default: { if (collisionShape->isConvex()) { btConvexShape* convex = (btConvexShape*)collisionShape; { btShapeHull* hull = new btShapeHull(convex); hull->buildHull(0.0, 1); { //int strideInBytes = 9*sizeof(float); //int numVertices = hull->numVertices(); //int numIndices =hull->numIndices(); for (int t = 0; t < hull->numTriangles(); t++) { btVector3 triNormal; int index0 = hull->getIndexPointer()[t * 3 + 0]; int index1 = hull->getIndexPointer()[t * 3 + 1]; int index2 = hull->getIndexPointer()[t * 3 + 2]; btVector3 pos0 = parentTransform * hull->getVertexPointer()[index0]; btVector3 pos1 = parentTransform * hull->getVertexPointer()[index1]; btVector3 pos2 = parentTransform * hull->getVertexPointer()[index2]; triNormal = (pos1 - pos0).cross(pos2 - pos0); triNormal.safeNormalize(); for (int v = 0; v < 3; v++) { int index = hull->getIndexPointer()[t * 3 + v]; btVector3 pos = parentTransform * hull->getVertexPointer()[index]; indicesOut.push_back(vertexPositions.size()); vertexPositions.push_back(pos); vertexNormals.push_back(triNormal); } } } delete hull; } } else { if (collisionShape->isCompound()) { btCompoundShape* compound = (btCompoundShape*)collisionShape; for (int i = 0; i < compound->getNumChildShapes(); i++) { btTransform childWorldTrans = parentTransform * compound->getChildTransform(i); CollisionShape2TriangleMesh(compound->getChildShape(i), childWorldTrans, vertexPositions, vertexNormals, indicesOut); } } else { if (collisionShape->getShapeType() == SDF_SHAPE_PROXYTYPE) { //not yet } else { btAssert(0); } } } } }; }
void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { (void)dispatchInfo; if (!m_manifoldPtr) return; const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); btTransform planeInConvex; planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); btTransform convexInPlaneTrans; convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); resultOut->setPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); } //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: //they keep on rolling forever because of the additional off-center contact points //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) { btVector3 v0,v1; btPlaneSpace1(planeNormal,v0,v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radius = convexShape->getAngularMotionDisc(); perturbeAngle = gContactBreakingThreshold / radius; if ( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btQuaternion perturbeRot(v0,perturbeAngle); for (int i=0;i<m_numPerturbationIterations;i++) { btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(planeNormal,iterationAngle); collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut); } } if (m_ownManifold) { if (m_manifoldPtr->getNumContacts()) { resultOut->refreshContactPoints(); } } }
void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) { // Draw a small simplex at the center of the object { btVector3 start = worldTransform.getOrigin(); getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0)); getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0)); getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1)); } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) { const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) { btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* colShape = compoundShape->getChildShape(i); debugDrawObject(worldTransform*childTrans,colShape,color); } } else { switch (shape->getShapeType()) { case SPHERE_SHAPE_PROXYTYPE: { const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin debugDrawSphere(radius, worldTransform, color); break; } case MULTI_SPHERE_SHAPE_PROXYTYPE: { const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) { btTransform childTransform = worldTransform; childTransform.getOrigin() += multiSphereShape->getSpherePosition(i); debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color); } break; } case CAPSULE_SHAPE_PROXYTYPE: { const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); btScalar radius = capsuleShape->getRadius(); btScalar halfHeight = capsuleShape->getHalfHeight(); // Draw the ends { btTransform childTransform = worldTransform; childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0); debugDrawSphere(radius, childTransform, color); } { btTransform childTransform = worldTransform; childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0); debugDrawSphere(radius, childTransform, color); } // Draw some additional lines btVector3 start = worldTransform.getOrigin(); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color); break; } case CONE_SHAPE_PROXYTYPE: { const btConeShape* coneShape = static_cast<const btConeShape*>(shape); btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); btScalar height = coneShape->getHeight();//+coneShape->getMargin(); btVector3 start = worldTransform.getOrigin(); int upAxis= coneShape->getConeUpIndex(); btVector3 offsetHeight(0,0,0); offsetHeight[upAxis] = height * btScalar(0.5); btVector3 offsetRadius(0,0,0); offsetRadius[(upAxis+1)%3] = radius; btVector3 offset2Radius(0,0,0); offset2Radius[(upAxis+2)%3] = radius; getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); break; } case CYLINDER_SHAPE_PROXYTYPE: { const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); int upAxis = cylinder->getUpAxis(); btScalar radius = cylinder->getRadius(); btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; btVector3 start = worldTransform.getOrigin(); btVector3 offsetHeight(0,0,0); offsetHeight[upAxis] = halfHeight; btVector3 offsetRadius(0,0,0); offsetRadius[(upAxis+1)%3] = radius; getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); break; } case STATIC_PLANE_PROXYTYPE: { const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); btScalar planeConst = staticPlaneShape->getPlaneConstant(); const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); btVector3 planeOrigin = planeNormal * planeConst; btVector3 vec0,vec1; btPlaneSpace1(planeNormal,vec0,vec1); btScalar vecLen = 100.f; btVector3 pt0 = planeOrigin + vec0*vecLen; btVector3 pt1 = planeOrigin - vec0*vecLen; btVector3 pt2 = planeOrigin + vec1*vecLen; btVector3 pt3 = planeOrigin - vec1*vecLen; getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color); getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color); break; } default: { if (shape->isConcave()) { btConcaveShape* concaveMesh = (btConcaveShape*) shape; //todo pass camera, for some culling btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); } if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) { btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; //todo: pass camera for some culling btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); //DebugDrawcallback drawCallback; DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); } /// for polyhedral shapes if (shape->isPolyhedral()) { btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; int i; for (i=0;i<polyshape->getNumEdges();i++) { btPoint3 a,b; polyshape->getEdge(i,a,b); btVector3 wa = worldTransform * a; btVector3 wb = worldTransform * b; getDebugDrawer()->drawLine(wa,wb,color); } } } } } }