btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
{

	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	// fixed axis in worldspace
	btVector3 rbAxisA1, rbAxisA2;
	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);

	m_rbAFrame.getOrigin() = pivotInA;
	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;

	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);


	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
	
	//start with free
	m_lowerLimit = btScalar(1e30);
	m_upperLimit = btScalar(-1e30);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;
}

void	btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
{

	btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
	btScalar radius = halfExtents.length();
	btVector3 center = (aabbMax + aabbMin) * btScalar(0.5);
	
	//this is where the triangles are generated, given AABB and plane equation (normal/constant)

	btVector3 tangentDir0,tangentDir1;

	//tangentDir0/tangentDir1 can be precalculated
	btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);

	btVector3 supVertex0,supVertex1;

	btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
	
	btVector3 triangle[3];
	triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
	triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius;
	triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius;

	callback->processTriangle(triangle,0,0);

	triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
	triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
	triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;

	callback->processTriangle(triangle,0,1);

}

btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0),m_limit()
{

	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	// fixed axis in worldspace
	btVector3 rbAxisA1, rbAxisA2;
	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);

	m_rbAFrame.getOrigin() = pivotInA;
	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;

	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);


	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
	
	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}

static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
	btVector3& normalOnB,
	btVector3& pointOnB,
	btScalar capsuleLengthA,
	btScalar capsuleRadiusA,
	btScalar capsuleLengthB,
	btScalar capsuleRadiusB,
	int capsuleAxisA,
	int capsuleAxisB,
	const btTransform& transformA,
	const btTransform& transformB,
	btScalar distanceThreshold)
{
	btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
	btVector3 translationA = transformA.getOrigin();
	btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
	btVector3 translationB = transformB.getOrigin();

	// translation between centers

	btVector3 translation = translationB - translationA;

	// compute the closest points of the capsule line segments

	btVector3 ptsVector;  // the vector between the closest points

	btVector3 offsetA, offsetB;  // offsets from segment centers to their closest points
	btScalar tA, tB;             // parameters on line segment

	segmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, translation,
						  directionA, capsuleLengthA, directionB, capsuleLengthB);

	btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;

	if (distance > distanceThreshold)
		return distance;

	btScalar lenSqr = ptsVector.length2();
	if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
	{
		//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
		btVector3 q;
		btPlaneSpace1(directionA, normalOnB, q);
	}
	else
	{
		// compute the contact normal
		normalOnB = ptsVector * -btRecipSqrt(lenSqr);
	}
	pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB;

	return distance;
}

void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
	(void)dispatchInfo;
	if (!m_manifoldPtr)
		return;

    btCollisionObject* convexObj = m_isSwapped? body1 : body0;
	btCollisionObject* planeObj = m_isSwapped? body0: body1;

	btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
	btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();

    
	const btVector3& planeNormal = planeShape->getPlaneNormal();
	//const btScalar& planeConstant = planeShape->getPlaneConstant();

	//first perform a collision query with the non-perturbated collision objects
	{
		btQuaternion rotq(0,0,0,1);
		collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut);
	}

	if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
	{
		btVector3 v0,v1;
		btPlaneSpace1(planeNormal,v0,v1);
		//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects

		const btScalar angleLimit = 0.125f * SIMD_PI;
		btScalar perturbeAngle;
		btScalar radius = convexShape->getAngularMotionDisc();
		perturbeAngle = gContactBreakingThreshold / radius;
		if ( perturbeAngle > angleLimit ) 
				perturbeAngle = angleLimit;

		btQuaternion perturbeRot(v0,perturbeAngle);
		for (int i=0;i<m_numPerturbationIterations;i++)
		{
			btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
			btQuaternion rotq(planeNormal,iterationAngle);
			collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut);
		}
	}

	if (m_ownManifold)
	{
		if (m_manifoldPtr->getNumContacts())
		{
			resultOut->refreshContactPoints();
		}
	}
}

btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false), m_enableAngularMotor(false), 
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{

	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	// fixed axis in worldspace
	btVector3 rbAxisA1, rbAxisA2;
	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);

	m_rbAFrame.getOrigin() = pivotInA;
	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;

	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);


	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
	
#ifndef	_BT_USE_CENTER_LIMIT_
	//start with free
	m_lowerLimit = btScalar(1.0f);
	m_upperLimit = btScalar(-1.0f);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;
#endif
	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}

void	btConeTwistConstraint::buildJacobian()
{
	if (m_useSolveConstraintObsolete)
	{
		m_appliedImpulse = btScalar(0.);
		m_accTwistLimitImpulse = btScalar(0.);
		m_accSwingLimitImpulse = btScalar(0.);
		m_accMotorImpulse = btVector3(0.,0.,0.);

		if (!m_angularOnly)
		{
			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
			btVector3 relPos = pivotBInW - pivotAInW;

			btVector3 normal[3];
			if (relPos.length2() > SIMD_EPSILON)
			{
				normal[0] = relPos.normalized();
			}
			else
			{
				normal[0].setValue(btScalar(1.0),0,0);
			}

			btPlaneSpace1(normal[0], normal[1], normal[2]);

			for (int i=0;i<3;i++)
			{
				new (&m_jac[i]) btJacobianEntry(
				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
				pivotAInW - m_rbA.getCenterOfMassPosition(),
				pivotBInW - m_rbB.getCenterOfMassPosition(),
				normal[i],
				m_rbA.getInvInertiaDiagLocal(),
				m_rbA.getInvMass(),
				m_rbB.getInvInertiaDiagLocal(),
				m_rbB.getInvMass());
			}
		}

		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
	}
}

void ConstraintPhysicsSetup::stepSimulation(float deltaTime)
{
	val = spDoorHinge->getAccumulatedHingeAngle() * SIMD_DEGS_PER_RAD;
	if (m_dynamicsWorld)
	{
		spDoorHinge->enableAngularMotor(true, targetVel, maxImpulse);

		m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);

		btHingeConstraint* hinge = spDoorHinge;

		if (hinge)
		{
			const btRigidBody& bodyA = hinge->getRigidBodyA();
			const btRigidBody& bodyB = hinge->getRigidBodyB();

			btTransform trA = bodyA.getWorldTransform();
			btVector3 angVelA = bodyA.getAngularVelocity();
			btVector3 angVelB = bodyB.getAngularVelocity();

			{
				btVector3 ax1 = trA.getBasis() * hinge->getFrameOffsetA().getBasis().getColumn(2);
				btScalar vel = angVelA.dot(ax1);
				vel -= angVelB.dot(ax1);
				printf("hinge velocity (q) = %f\n", vel);
				actualHingeVelocity = vel;
			}
			btVector3 ortho0, ortho1;
			btPlaneSpace1(btAxisA, ortho0, ortho1);
			{
				btScalar vel2 = angVelA.dot(ortho0);
				vel2 -= angVelB.dot(ortho0);
				printf("hinge orthogonal1 velocity (q) = %f\n", vel2);
			}
			{
				btScalar vel0 = angVelA.dot(ortho1);
				vel0 -= angVelB.dot(ortho1);
				printf("hinge orthogonal0 velocity (q) = %f\n", vel0);
			}
		}
	}
}

void terrain::drawPlane(float x, float y)
{
    const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(m_planeBody->getCollisionShape());
    btVector3 normal = staticPlaneShape->getPlaneNormal();

   //btVector3 planeOrigin = normal * constant;
    btVector3 planeOrigin = m_planeBody->getCenterOfMassPosition();
    btVector3 vec0,vec1;
    btPlaneSpace1(normal,vec0,vec1);
    btVector3 pt0 = planeOrigin + (vec0*y + vec1*x);
    btVector3 pt1 = planeOrigin - (vec0*y + vec1*x);
    btVector3 pt2 = planeOrigin + (vec0*y - vec1*x);
    btVector3 pt3 = planeOrigin - (vec0*y - vec1*x);

    glNormal3f(normal.x(),normal.y(),normal.z());
    glBegin(GL_QUADS);
    glVertex3f(pt0.getX(),pt0.getY(),pt0.getZ());
    glVertex3f(pt3.getX(),pt3.getY(),pt3.getZ());
    glVertex3f(pt1.getX(),pt1.getY(),pt1.getZ());
    glVertex3f(pt2.getX(),pt2.getY(),pt2.getZ());
    glEnd();
}

void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
{
    btAssert(m_manifoldPtr);
    //order in manifold needs to match

    if (depth > m_manifoldPtr->getContactBreakingThreshold())
//	if (depth > m_manifoldPtr->getContactProcessingThreshold())
        return;

    bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();

    btVector3 pointA = pointInWorld + normalOnBInWorld * depth;

    btVector3 localA;
    btVector3 localB;

    if (isSwapped)
    {
        localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
        localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
    } else
    {
        localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
        localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
    }

    btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth);
    newPt.m_positionWorldOnA = pointA;
    newPt.m_positionWorldOnB = pointInWorld;

    int insertIndex = m_manifoldPtr->getCacheEntry(newPt);

    newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
    newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
    newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
    btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2);



    //BP mod, store contact triangles.
    if (isSwapped)
    {
        newPt.m_partId0 = m_partId1;
        newPt.m_partId1 = m_partId0;
        newPt.m_index0  = m_index1;
        newPt.m_index1  = m_index0;
    } else
    {
        newPt.m_partId0 = m_partId0;
        newPt.m_partId1 = m_partId1;
        newPt.m_index0  = m_index0;
        newPt.m_index1  = m_index1;
    }

    //printf("depth=%f\n", depth);
    ///@todo, check this for any side effects
    if (insertIndex >= 0)
    {
        //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
        m_manifoldPtr->replaceContactPoint(newPt, insertIndex);
    } else
    {
        insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
    }

    //User can override friction and/or restitution
    // DrChat: Removed for multithreading version
    /*
    if (gContactAddedCallback &&
    	//and if either of the two bodies requires custom material
    	 ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
    	   (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
    {
    	//experimental feature info, for per-triangle material etc.
    	const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
    	const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
    	(*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1);
    }
    */
}

//.........这里部分代码省略.........
					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);

void	btHingeConstraint::buildJacobian()
{
	if (m_useSolveConstraintObsolete)
	{
		m_appliedImpulse = btScalar(0.);
		m_accMotorImpulse = btScalar(0.);

		if (!m_angularOnly)
		{
			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
			btVector3 relPos = pivotBInW - pivotAInW;

			btVector3 normal[3];
			if (relPos.length2() > SIMD_EPSILON)
			{
				normal[0] = relPos.normalized();
			}
			else
			{
				normal[0].setValue(btScalar(1.0),0,0);
			}

			btPlaneSpace1(normal[0], normal[1], normal[2]);

			for (int i=0;i<3;i++)
			{
				new (&m_jac[i]) btJacobianEntry(
				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
				pivotAInW - m_rbA.getCenterOfMassPosition(),
				pivotBInW - m_rbB.getCenterOfMassPosition(),
				normal[i],
				m_rbA.getInvInertiaDiagLocal(),
				m_rbA.getInvMass(),
				m_rbB.getInvInertiaDiagLocal(),
				m_rbB.getInvMass());
			}
		}

		//calculate two perpendicular jointAxis, orthogonal to hingeAxis
		//these two jointAxis require equal angular velocities for both bodies

		//this is unused for now, it's a todo
		btVector3 jointAxis0local;
		btVector3 jointAxis1local;
		
		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);

		btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
		btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
		btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
			
		new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
			m_rbA.getInvInertiaDiagLocal(),
			m_rbB.getInvInertiaDiagLocal());

		new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
			m_rbA.getInvInertiaDiagLocal(),
			m_rbB.getInvInertiaDiagLocal());

		new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
			m_rbA.getInvInertiaDiagLocal(),
			m_rbB.getInvInertiaDiagLocal());

			// clear accumulator
			m_accLimitImpulse = btScalar(0.);

			// test angular limit
			testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());

		//Compute K = J*W*J' for hinge axis
		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
		m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
							 getRigidBodyB().computeAngularImpulseDenominator(axisA));

	}
}

static
__inline
void solveFriction(Constraint4& cs,
                   const float4& posA, float4& linVelA, float4& angVelA, float invMassA, const Matrix3x3& invInertiaA,
                   const float4& posB, float4& linVelB, float4& angVelB, float invMassB, const Matrix3x3& invInertiaB,
                   float maxRambdaDt[4], float minRambdaDt[4])
{
    if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
    const float4& center = cs.m_center;

    float4 n = -cs.m_linear;

    float4 tangent[2];
#if 1
    btPlaneSpace1 (&n, &tangent[0],&tangent[1]);
#else
    float4 r = cs.m_worldPos[0]-center;
    tangent[0] = cross3( n, r );
    tangent[1] = cross3( tangent[0], n );
    tangent[0] = normalize3( tangent[0] );
    tangent[1] = normalize3( tangent[1] );
#endif

    float4 angular0, angular1, linear;
    float4 r0 = center - posA;
    float4 r1 = center - posB;
    for(int i=0; i<2; i++)
    {
        setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
        float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
                                    linVelA, angVelA, linVelB, angVelB );
        rambdaDt *= cs.m_fJacCoeffInv[i];

        {
            float prevSum = cs.m_fAppliedRambdaDt[i];
            float updated = prevSum;
            updated += rambdaDt;
            updated = max2( updated, minRambdaDt[i] );
            updated = min2( updated, maxRambdaDt[i] );
            rambdaDt = updated - prevSum;
            cs.m_fAppliedRambdaDt[i] = updated;
        }

        float4 linImp0 = invMassA*linear*rambdaDt;
        float4 linImp1 = invMassB*(-linear)*rambdaDt;
        float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
        float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
#ifdef _WIN32
        btAssert(_finite(linImp0.x));
        btAssert(_finite(linImp1.x));
#endif
        linVelA += linImp0;
        angVelA += angImp0;
        linVelB += linImp1;
        angVelB += angImp1;
    }

    {   //	angular damping for point constraint
        float4 ab = normalize3( posB - posA );
        float4 ac = normalize3( center - posA );
        if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
        {
            float angNA = dot3F4( n, angVelA );
            float angNB = dot3F4( n, angVelB );

            angVelA -= (angNA*0.1f)*n;
            angVelB -= (angNB*0.1f)*n;
        }
    }
}

void kSetupContact(btParallelConstraintSolver* pSolver, 
				  btParallelConstraintSolverSetupTaskParams* pParams, 
				  btContactSolverInfo* pInfoGlobal, int threadId)
{
	int numConstraints = pParams[threadId].m_numContactConstraints;
	unsigned long int timeStamp;
	int startIndex = pParams[threadId].m_startIndex;
	btContactSolverInfo& infoGlobal = *pInfoGlobal;
	for(int i = 0; i < numConstraints; i++)
	{
		timeStamp = sClock.getTimeMicroseconds();
		btSolverConstraint& solverConstraint = pSolver->m_tmpSolverContactConstraintPool[startIndex + i];
		solverConstraint.m_numConsecutiveRowsPerKernel = timeStamp;
		btCollisionObject* colObj0 = (btCollisionObject*)solverConstraint.m_solverBodyA;
		btCollisionObject* colObj1 = (btCollisionObject*)solverConstraint.m_solverBodyB;
		btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
		btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
		btManifoldPoint& cp = *((btManifoldPoint*)(solverConstraint.m_originalContactPoint));
		btVector3 rel_pos1;
		btVector3 rel_pos2;
		btScalar relaxation;
		btScalar rel_vel;
		btVector3 vel;
		pSolver->setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
		int currFrictIndex = solverConstraint.m_frictionIndex;
		if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
		{
			cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
			btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
			if(!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
			{
				cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
				if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
				{
					cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
					cp.m_lateralFrictionDir2.normalize();//??
					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
					btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
					currFrictIndex++;
					pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
				}
				applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
				applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
				btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
				currFrictIndex++;
				pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
				cp.m_lateralFrictionInitialized = true;
			} 
			else
			{
				//re-calculate friction direction every frame, todo: check if this is really needed
				btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
				{
					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
					btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
					currFrictIndex++;
					pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
				}
				applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
				applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
				btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
				currFrictIndex++;
				pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
				cp.m_lateralFrictionInitialized = true;
			}
		} 
		else
		{
			btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
			currFrictIndex++;
			pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
			{
				btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
				currFrictIndex++;
				pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
			}
		}
		pSolver->setFrictionConstraintImpulse( solverConstraint, solverBodyA, solverBodyB, cp, infoGlobal);
	}
}

//.........这里部分代码省略.........
	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)
			{

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;
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
				}
				
				
				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;