Пример #1
0
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();
		}
	}
}
Пример #6
0
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());
	}
}
Пример #8
0
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);
			}
		}
	}
}
Пример #9
0
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();
}
Пример #10
0
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);
    }
    */
}
Пример #11
0
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));

	}
}
Пример #13
0
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);
	}
}
Пример #15
0
//
// 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();
	}

}
Пример #16
0
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();
	}
}
Пример #19
0
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;
}
Пример #20
0
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);

					}

					
				}
			}
		}
	}
}