void btMultiBody::stepVelocities(btScalar dt,
                               btAlignedObjectArray<btScalar> &scratch_r,
                               btAlignedObjectArray<btVector3> &scratch_v,
                               btAlignedObjectArray<btMatrix3x3> &scratch_m)
{
    // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
    // and the base linear & angular accelerations.

    // We apply damping forces in this routine as well as any external forces specified by the 
    // caller (via addBaseForce etc).

    // output should point to an array of 6 + num_links reals.
    // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
    // num_links joint acceleration values.
    
	int num_links = getNumLinks();

    const btScalar DAMPING_K1_LINEAR = m_linearDamping;
	const btScalar DAMPING_K2_LINEAR = m_linearDamping;

	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
	const btScalar DAMPING_K2_ANGULAR= m_angularDamping;

    btVector3 base_vel = getBaseVel();
    btVector3 base_omega = getBaseOmega();

    // Temporary matrices/vectors -- use scratch space from caller
    // so that we don't have to keep reallocating every frame

    scratch_r.resize(2*num_links + 6);
    scratch_v.resize(8*num_links + 6);
    scratch_m.resize(4*num_links + 4);

    btScalar * r_ptr = &scratch_r[0];
    btScalar * output = &scratch_r[num_links];  // "output" holds the q_double_dot results
    btVector3 * v_ptr = &scratch_v[0];
    
    // vhat_i  (top = angular, bottom = linear part)
    btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
    btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;

    // zhat_i^A
    btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
    btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;

    // chat_i  (note NOT defined for the base)
    btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
    btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;

    // top left, top right and bottom left blocks of Ihat_i^A.
    // bottom right block = transpose of top left block and is not stored.
    // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
    btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
    btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
    btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];

    // Cached 3x3 rotation matrices from parent frame to this frame.
    btMatrix3x3 * rot_from_parent = &matrix_buf[0];
    btMatrix3x3 * rot_from_world = &scratch_m[0];

    // hhat_i, ahat_i
    // hhat is NOT stored for the base (but ahat is)
    btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
    btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
    btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
    btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;

    // Y_i, D_i
    btScalar * Y = r_ptr; r_ptr += num_links;
    btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;

    // ptr to the joint accel part of the output
    btScalar * joint_accel = output + 6;


    // Start of the algorithm proper.
    
    // First 'upward' loop.
    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.

    rot_from_parent[0] = btMatrix3x3(base_quat);

    vel_top_angular[0] = rot_from_parent[0] * base_omega;
    vel_bottom_linear[0] = rot_from_parent[0] * base_vel;

    if (fixed_base) {
        zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
    } else {
        zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force 
                                                   - base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
		
        zero_acc_bottom_linear[0] =
            - (rot_from_parent[0] * base_torque);

		if (m_useGyroTerm)
			zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] );

        zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());

    }
//.........这里部分代码省略.........

void BulletConstraintSolver()
{
	btPgsSolver pgs;
	btContactSolverInfo info;
	rbs.resize(0);

	for (int i=0;i<numRigidBodies;i++)
	{
		btRigidBody& rb = rbs.expandNonInitializing();

		rb.m_companionId=-1;
		rb.m_angularFactor.setValue(1,1,1);
		rb.m_anisotropicFriction.setValue(1,1,1);
		
		rb.m_invMass = bodies[i].getMassInv();
		rb.m_linearFactor.setValue(1,1,1);
		btVector3 pos(states[i].getPosition().getX(),states[i].getPosition().getY(),states[i].getPosition().getZ());
		rb.m_worldTransform.setIdentity();
		btQuaternion orn(states[i].getOrientation().getX(),states[i].getOrientation().getY(),states[i].getOrientation().getZ(),states[i].getOrientation().getW());
		rb.m_worldTransform.setRotation(orn);
		rb.m_worldTransform.setOrigin(pos);
		PfxMatrix3 ori(states[i].getOrientation());
		rb.m_worldTransform.setRotation(orn);

		PfxMatrix3 inertiaInvWorld = ori * bodies[i].getInertiaInv() * transpose(ori);
		rb.m_invInertiaWorld.setIdentity();

		if (rb.m_invMass)
		{
			for (int row=0;row<3;row++)
			{
				for (int col=0;col<3;col++)
				{
					rb.m_invInertiaWorld[col][row] = inertiaInvWorld.getElem(col,row);
				}
			}
		} else
		{
			rb.m_invInertiaWorld = btMatrix3x3(0,0,0,0,0,0,0,0,0);
		}
		rb.m_linearVelocity.setValue(states[i].getLinearVelocity().getX(),states[i].getLinearVelocity().getY(),states[i].getLinearVelocity().getZ());
		rb.m_angularVelocity.setValue(states[i].getAngularVelocity().getX(),states[i].getAngularVelocity().getY(),states[i].getAngularVelocity().getZ());
//		printf("body added\n");
	}

	btAlignedObjectArray<btCollisionObject*> bodyPtrs;
	bodyPtrs.resize(rbs.size());
	for (int i=0;i<rbs.size();i++)
	{
		bodyPtrs[i] = &rbs[i];
	}


	unsigned int numCurrentPairs = numPairs[pairSwap];
	PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];

	PfxSetupContactConstraintsParam param;
	param.contactPairs = currentPairs;
	param.numContactPairs = numCurrentPairs;
	param.offsetContactManifolds = contacts;
	param.offsetRigidStates = states;
	param.offsetRigidBodies = bodies;
	param.offsetSolverBodies = solverBodies;
	param.numRigidBodies = numRigidBodies;
	param.timeStep = timeStep;
	param.separateBias = separateBias;

	BulletSetupContactConstraints(param);

	btAlignedObjectArray<btPersistentManifold*> manifoldPtrs;
	manifoldPtrs.resize(manifolds.size());

	for (int i=0;i<manifolds.size();i++)
	{
		manifoldPtrs[i] = &manifolds[i];
	}

	if (bodyPtrs.size() && manifoldPtrs.size())
	{
		pgs.solveGroup(&bodyPtrs[0],bodyPtrs.size(),&manifoldPtrs[0],manifoldPtrs.size(),0,0,info,0,0,0);

		for (int i=0;i<numRigidBodies;i++)
		{
			btVector3 linvel = rbs[i].getLinearVelocity();
			btVector3 angvel = rbs[i].getAngularVelocity();
			states[i].setLinearVelocity(PfxVector3(linvel.getX(),linvel.getY(),linvel.getZ()));
			states[i].setAngularVelocity(PfxVector3(angvel.getX(),angvel.getY(),angvel.getZ()));
		}
	}

	BulletWriteWarmstartContactConstraints(param);



}

void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,  const btMatrix4x4& parentTransMat)
{
	const char* nodeName = node->Attribute("id");
	//printf("processing node %s\n", nodeName);

	
	btMatrix4x4 nodeTrans;
	nodeTrans.setIdentity();

	///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
	{
		for (TiXmlElement* transElem = node->FirstChildElement("matrix");transElem;transElem=node->NextSiblingElement("matrix"))
		{
			if (transElem->GetText())
			{
				btAlignedObjectArray<float> floatArray;
				TokenFloatArray adder(floatArray);
				tokenize(transElem->GetText(),adder);
				if (floatArray.size()==16)
				{
					btMatrix4x4 t(floatArray[0],floatArray[1],floatArray[2],floatArray[3],
									floatArray[4],floatArray[5],floatArray[6],floatArray[7],
									floatArray[8],floatArray[9],floatArray[10],floatArray[11],
									floatArray[12],floatArray[13],floatArray[14],floatArray[15]);

					nodeTrans = nodeTrans*t;
				} else
				{
					b3Warning("Error: expected 16 elements in a <matrix> element, skipping\n");
				}
			}
		}
	}

	{
		for (TiXmlElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate"))
		{
			if (transElem->GetText())
			{
				btVector3 pos = getVector3FromXmlText(transElem->GetText());
				//nodePos+= unitScaling*parentScaling*pos;
				btMatrix4x4 t;
				t.setPureTranslation(pos);
				nodeTrans = nodeTrans*t;

			}
		}
	}
	{
		for(TiXmlElement* scaleElem = node->FirstChildElement("scale");
				scaleElem!= NULL; scaleElem= node->NextSiblingElement("scale")) 
		{
			if (scaleElem->GetText())
			{
				btVector3 scaling = getVector3FromXmlText(scaleElem->GetText());
				btMatrix4x4 t;
				t.setPureScaling(scaling);
				nodeTrans = nodeTrans*t;
			}
		}
	}
	{
		for(TiXmlElement* rotateElem = node->FirstChildElement("rotate");
				rotateElem!= NULL; rotateElem= node->NextSiblingElement("rotate")) 
		{
			if (rotateElem->GetText())
			{
				//accumulate orientation
				btVector4 rotate = getVector4FromXmlText(rotateElem->GetText());
				btQuaternion orn(btVector3(rotate),btRadians(rotate[3]));//COLLADA DAE rotate is in degrees, convert to radians
				btMatrix4x4 t;
				t.setPureRotation(orn);
				nodeTrans = nodeTrans*t;
			}
		}
	}
	
	nodeTrans = parentTransMat*nodeTrans;
	
	for (TiXmlElement* instanceGeom = node->FirstChildElement("instance_geometry");
				instanceGeom!=0;
				instanceGeom=instanceGeom->NextSiblingElement("instance_geometry"))
	{
		const char* geomUrl = instanceGeom->Attribute("url");
		//printf("node referring to geom %s\n", geomUrl);
		geomUrl++;
		int* shapeIndexPtr = name2Shape[geomUrl];
		if (shapeIndexPtr)
		{
		//	int index = *shapeIndexPtr;
			//printf("found geom with index %d\n", *shapeIndexPtr);
			ColladaGraphicsInstance& instance = visualShapeInstances.expand();
			instance.m_shapeIndex = *shapeIndexPtr;
			instance.m_worldTransform = nodeTrans;
		} else
		{
			b3Warning("geom not found\n");
		}
	}

//.........这里部分代码省略.........

static inline T				average(const btAlignedObjectArray<T>& items)
{
	const btScalar	n=(btScalar)(items.size()>0?items.size():1);
	return(sum(items)/n);
}

/**
 * Create a sequence of flag objects and add them to the world.
 */
void createFlag( int width, int height, btAlignedObjectArray<btSoftBody *> &flags )
{
	// First create a triangle mesh to represent a flag

	using namespace BTAcceleratedSoftBody;	
	using Vectormath::Aos::Matrix3;
	using Vectormath::Aos::Vector3;

	// Allocate a simple mesh consisting of a vertex array and a triangle index array
	btIndexedMesh mesh;
	mesh.m_numVertices = width*height;
	mesh.m_numTriangles = 2*(width-1)*(height-1);

	btVector3 *vertexArray = new btVector3[mesh.m_numVertices];

	mesh.m_vertexBase = reinterpret_cast<const unsigned char*>(vertexArray);
	int *triangleVertexIndexArray = new int[3*mesh.m_numTriangles];	
	mesh.m_triangleIndexBase = reinterpret_cast<const unsigned char*>(triangleVertexIndexArray);
	mesh.m_triangleIndexStride = sizeof(int)*3;
	mesh.m_vertexStride = sizeof(Vector3);

	// Generate normalised object space vertex coordinates for a rectangular flag
	float zCoordinate = 0.0f;
	
	Matrix3 defaultScale(Vector3(5.f, 0.f, 0.f), Vector3(0.f, 20.f, 0.f), Vector3(0.f, 0.f, 1.f));
	for( int y = 0; y < height; ++y )
	{
		float yCoordinate = y*2.0f/float(height) - 1.0f;
		for( int x = 0; x < width; ++x )
		{			
			float xCoordinate = x*2.0f/float(width) - 1.0f;

			Vector3 vertex(xCoordinate, yCoordinate, zCoordinate);
			Vector3 transformedVertex = defaultScale*vertex;

			vertexArray[y*width + x] = btVector3(transformedVertex.getX(), transformedVertex.getY(), transformedVertex.getZ() );

		}
	}

	// Generate vertex indices for triangles
	for( int y = 0; y < (height-1); ++y )
	{
		for( int x = 0; x < (width-1); ++x )
		{	
			// Triangle 0
			// Top left of square on mesh
			{
				int vertex0 = y*width + x;
				int vertex1 = vertex0 + 1;
				int vertex2 = vertex0 + width;
				int triangleIndex = 2*y*(width-1) + 2*x;
				triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)] = vertex0;
				triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex+1)/sizeof(int)+1] = vertex1;
				triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex+2)/sizeof(int)+2] = vertex2;
			}

			// Triangle 1
			// Bottom right of square on mesh
			{
				int vertex0 = y*width + x + 1;
				int vertex1 = vertex0 + width;
				int vertex2 = vertex1 - 1;
				int triangleIndex = 2*y*(width-1) + 2*x + 1;
				triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)] = vertex0;
				triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)+1] = vertex1;
				triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)+2] = vertex2;
			}
		}
	}

	
	float rotateAngleRoundZ = 0.5;
	float rotateAngleRoundX = 0.5;
	btMatrix3x3 defaultRotate;
	defaultRotate[0] = btVector3(cos(rotateAngleRoundZ), sin(rotateAngleRoundZ), 0.f); 
	defaultRotate[1] = btVector3(-sin(rotateAngleRoundZ), cos(rotateAngleRoundZ), 0.f);
	defaultRotate[2] = btVector3(0.f, 0.f, 1.f);


	//btMatrix3x3 defaultRotateAndScale( (defaultRotateX*defaultRotate) );
#ifdef TABLETEST
	btMatrix3x3 defaultRotateX;
	rotateAngleRoundX = 3.141592654/2;
	defaultRotateX[0] = btVector3(1.f, 0.f, 0.f);
	defaultRotateX[1] = btVector3( 0.f, cos(rotateAngleRoundX), sin(rotateAngleRoundX));
	defaultRotateX[2] = btVector3(0.f, -sin(rotateAngleRoundX), cos(rotateAngleRoundX));
	btMatrix3x3 defaultRotateAndScale( (defaultRotateX) );
#else
	btMatrix3x3 defaultRotateX;
	defaultRotateX[0] = btVector3(1.f, 0.f, 0.f);
	defaultRotateX[1] = btVector3( 0.f, cos(rotateAngleRoundX), sin(rotateAngleRoundX));
	defaultRotateX[2] = btVector3(0.f, -sin(rotateAngleRoundX), cos(rotateAngleRoundX));
	btMatrix3x3 defaultRotateAndScale( (defaultRotateX) );
#endif


//.........这里部分代码省略.........

void VoronoiFractureDemo::voronoiConvexHullShatter(const btAlignedObjectArray<btVector3>& points, const btAlignedObjectArray<btVector3>& verts, const btQuaternion& bbq, const btVector3& bbt, btScalar matDensity) {
	// points define voronoi cells in world space (avoid duplicates)
	// verts = source (convex hull) mesh vertices in local space
	// bbq & bbt = source (convex hull) mesh quaternion rotation and translation
	// matDensity = Material density for voronoi shard mass calculation
	btConvexHullComputer* convexHC = new btConvexHullComputer();
	btAlignedObjectArray<btVector3> vertices, chverts;
	btVector3 rbb, nrbb;
	btScalar nlength, maxDistance, distance;
	btAlignedObjectArray<btVector3> sortedVoronoiPoints;
	sortedVoronoiPoints.copyFromArray(points);
	btVector3 normal, plane;
	btAlignedObjectArray<btVector3> planes, convexPlanes;
	std::set<int> planeIndices;
	std::set<int>::iterator planeIndicesIter;
	int numplaneIndices;
	int cellnum = 0;
	int i, j, k;

	// Convert verts to world space and get convexPlanes
	int numverts = verts.size();
	chverts.resize(verts.size());
	for (i=0; i < numverts ;i++) {
		chverts[i] = quatRotate(bbq, verts[i]) + bbt;
	}
	//btGeometryUtil::getPlaneEquationsFromVertices(chverts, convexPlanes);
	// Using convexHullComputer faster than getPlaneEquationsFromVertices for large meshes...
	convexHC->compute(&chverts[0].getX(), sizeof(btVector3), numverts, 0.0, 0.0);
	int numFaces = convexHC->faces.size();
	int v0, v1, v2; // vertices
	for (i=0; i < numFaces; i++) {
		const btConvexHullComputer::Edge* edge = &convexHC->edges[convexHC->faces[i]];
		v0 = edge->getSourceVertex();
		v1 = edge->getTargetVertex();
		edge = edge->getNextEdgeOfFace();
		v2 = edge->getTargetVertex();
		plane = (convexHC->vertices[v1]-convexHC->vertices[v0]).cross(convexHC->vertices[v2]-convexHC->vertices[v0]).normalize();
		plane[3] = -plane.dot(convexHC->vertices[v0]);
		convexPlanes.push_back(plane);
	}
	const int numconvexPlanes = convexPlanes.size();

	int numpoints = points.size();
	for (i=0; i < numpoints ;i++) {
		curVoronoiPoint = points[i];
		planes.copyFromArray(convexPlanes);
		for (j=0; j < numconvexPlanes ;j++) {
			planes[j][3] += planes[j].dot(curVoronoiPoint);
		}
		maxDistance = SIMD_INFINITY;
		sortedVoronoiPoints.heapSort(pointCmp());
		for (j=1; j < numpoints; j++) {
			normal = sortedVoronoiPoints[j] - curVoronoiPoint;
			nlength = normal.length();
			if (nlength > maxDistance)
				break;
			plane = normal.normalized();
			plane[3] = -nlength / btScalar(2.);
			planes.push_back(plane);
			getVerticesInsidePlanes(planes, vertices, planeIndices);
			if (vertices.size() == 0)
				break;
			numplaneIndices = planeIndices.size();
			if (numplaneIndices != planes.size()) {
				planeIndicesIter = planeIndices.begin();
				for (k=0; k < numplaneIndices; k++) {
					if (k != *planeIndicesIter)
						planes[k] = planes[*planeIndicesIter];
					planeIndicesIter++;
				}
				planes.resize(numplaneIndices);
			}
			maxDistance = vertices[0].length();
			for (k=1; k < vertices.size(); k++) {
				distance = vertices[k].length();
				if (maxDistance < distance)
					maxDistance = distance;
			}
			maxDistance *= btScalar(2.);
		}
		if (vertices.size() == 0)
			continue;

		// Clean-up voronoi convex shard vertices and generate edges & faces
		convexHC->compute(&vertices[0].getX(), sizeof(btVector3), vertices.size(),0.0,0.0);

		// At this point we have a complete 3D voronoi shard mesh contained in convexHC

		// Calculate volume and center of mass (Stan Melax volume integration)
		numFaces = convexHC->faces.size();
		btScalar volume = btScalar(0.);
		btVector3 com(0., 0., 0.);
		for (j=0; j < numFaces; j++) {
			const btConvexHullComputer::Edge* edge = &convexHC->edges[convexHC->faces[j]];
			v0 = edge->getSourceVertex();
			v1 = edge->getTargetVertex();
			edge = edge->getNextEdgeOfFace();
			v2 = edge->getTargetVertex();
			while (v2 != v0) {
				// Counter-clockwise triangulated voronoi shard mesh faces (v0-v1-v2) and edges here...
//.........这里部分代码省略.........

///@todo: this is random access, it can be walked 'cache friendly'!
void btSimulationIslandManager::buildAndProcessIslands( btDispatcher* dispatcher,
                                                        btCollisionWorld* collisionWorld,
                                                        btAlignedObjectArray<btTypedConstraint*>& constraints,
                                                        IslandCallback* callback
                                                        )
{
	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();

	buildIslands(dispatcher,collisionWorld);

	BT_PROFILE("processIslands");

	if(!m_splitIslands)
	{
        btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
        int maxNumManifolds = dispatcher->getNumManifolds();

        for ( int i = 0; i < maxNumManifolds; i++ )
        {
            btPersistentManifold* manifold = manifolds[ i ];

            const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>( manifold->getBody0() );
            const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>( manifold->getBody1() );

            ///@todo: check sleeping conditions!
            if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) ||
                 ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) )
            {

                //kinematic objects don't merge islands, but wake up all connected objects
                if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING )
                {
                    if ( colObj0->hasContactResponse() )
                        colObj1->activate();
                }
                if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING )
                {
                    if ( colObj1->hasContactResponse() )
                        colObj0->activate();
                }
            }
        }
        btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL;
		callback->processIsland(&collisionObjects[0],
                                 collisionObjects.size(),
                                 manifolds,
                                 maxNumManifolds,
                                 constraintsPtr,
                                 constraints.size(),
                                 -1
                                 );
	}
	else
	{
        initIslandPools();

        //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
        addBodiesToIslands( collisionWorld );
        addManifoldsToIslands( dispatcher );
        addConstraintsToIslands( constraints );

        // m_activeIslands array should now contain all non-sleeping Islands, and each Island should
        // have all the necessary bodies, manifolds and constraints.

        // if we want to merge islands with small batch counts,
        if ( m_minimumSolverBatchSize > 1 )
        {
            mergeIslands();
        }
        // dispatch islands to solver
        m_islandDispatch( &m_activeIslands, callback );
	}
}

void btMultiBody::fillContactJacobian(int link,
                                    const btVector3 &contact_point,
                                    const btVector3 &normal,
                                    btScalar *jac,
                                    btAlignedObjectArray<btScalar> &scratch_r,
                                    btAlignedObjectArray<btVector3> &scratch_v,
                                    btAlignedObjectArray<btMatrix3x3> &scratch_m) const
{
    // temporary space
	int num_links = getNumLinks();
    scratch_v.resize(2*num_links + 2);
    scratch_m.resize(num_links + 1);

    btVector3 * v_ptr = &scratch_v[0];
    btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
    btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());

    scratch_r.resize(num_links);
    btScalar * results = num_links > 0 ? &scratch_r[0] : 0;

    btMatrix3x3 * rot_from_world = &scratch_m[0];

    const btVector3 p_minus_com_world = contact_point - base_pos;

    rot_from_world[0] = btMatrix3x3(base_quat);

    p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
    n_local[0] = rot_from_world[0] * normal;
    
    // omega coeffients first.
    btVector3 omega_coeffs;
    omega_coeffs = p_minus_com_world.cross(normal);
	jac[0] = omega_coeffs[0];
	jac[1] = omega_coeffs[1];
	jac[2] = omega_coeffs[2];
    // then v coefficients
    jac[3] = normal[0];
    jac[4] = normal[1];
    jac[5] = normal[2];

    // Set remaining jac values to zero for now.
    for (int i = 6; i < 6 + num_links; ++i) {
        jac[i] = 0;
    }

    // Qdot coefficients, if necessary.
    if (num_links > 0 && link > -1) {

        // TODO: speed this up -- don't calculate for links we don't need.
        // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
        // which is resulting in repeated work being done...)

        // calculate required normals & positions in the local frames.
        for (int i = 0; i < num_links; ++i) {

            // transform to local frame
            const int parent = links[i].parent;
            const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this);
            rot_from_world[i+1] = mtx * rot_from_world[parent+1];

            n_local[i+1] = mtx * n_local[parent+1];
            p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector;

            // calculate the jacobian entry
            if (links[i].is_revolute) {
                results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom );
            } else {
                results[i] = n_local[i+1].dot( links[i].axis_bottom );
            }
        }

        // Now copy through to output.
        while (link != -1) {
            jac[6 + link] = results[link];
            link = links[link].parent;
        }
    }
}

void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
                                       btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
{
    // Temporary matrices/vectors -- use scratch space from caller
    // so that we don't have to keep reallocating every frame
	int num_links = getNumLinks();
    scratch_r.resize(num_links);
    scratch_v.resize(4*num_links + 4);

    btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
    btVector3 * v_ptr = &scratch_v[0];
    
    // zhat_i^A (scratch space)
    btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
    btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;

    // rot_from_parent (cached from calcAccelerations)
    const btMatrix3x3 * rot_from_parent = &matrix_buf[0];

    // hhat (cached), accel (scratch)
    const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
    const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
    btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
    btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;

    // Y_i (scratch), D_i (cached)
    btScalar * Y = r_ptr; r_ptr += num_links;
    const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;

    btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());


    
    // First 'upward' loop.
    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.

    btVector3 input_force(force[3],force[4],force[5]);
    btVector3 input_torque(force[0],force[1],force[2]);
    
    // Fill in zero_acc
    // -- set to force/torque on the base, zero otherwise
    if (fixed_base) 
	{
        zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
    } else 
	{
        zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
        zero_acc_bottom_linear[0] =  - (rot_from_parent[0] * input_torque);
    }
    for (int i = 0; i < num_links; ++i) 
	{
        zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
    }

    // 'Downward' loop.
    for (int i = num_links - 1; i >= 0; --i) 
	{

        Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
        Y[i] += force[6 + i];  // add joint torque
        
        const int parent = links[i].parent;
        
        // Zp += pXi * (Zi + hi*Yi/Di)
        btVector3 in_top, in_bottom, out_top, out_bottom;
        const btScalar Y_over_D = Y[i] / D[i];
        in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
        in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
        InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
                                in_top, in_bottom, out_top, out_bottom);
        zero_acc_top_angular[parent+1] += out_top;
        zero_acc_bottom_linear[parent+1] += out_bottom;
    }

    // ptr to the joint accel part of the output
    btScalar * joint_accel = output + 6;

    // Second 'upward' loop
    if (fixed_base) 
	{
        accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
    } else 
	{
		btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
		btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
		
		float result[6];
        solveImatrix(rhs_top,rhs_bot, result);
	//	printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);

        for (int i = 0; i < 3; ++i) {
            accel_top[0][i] = -result[i];
            accel_bottom[0][i] = -result[i+3];
        }

    }
    
    // now do the loop over the links
    for (int i = 0; i < num_links; ++i) {
//.........这里部分代码省略.........

	virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
	{

		//calc centroid, to shift vertices around center of mass
		btVector3 centroid(0,0,0);
		btAlignedObjectArray<btVector3> vertices;

		if(m_transformSubShapes)
		{

			//const unsigned int *src = result.mHullIndices;
			for (unsigned int i=0; i<result.mHullVcount; i++)
			{
				btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);

				centroid += vertex;

			}
			centroid *= 1.f/(float(result.mHullVcount) );
		}

		// collect vertices
		for (unsigned int i=0; i<result.mHullVcount; i++)
		{
			btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);

			if(m_transformSubShapes)
			{
				vertex -= centroid ;
			}
			vertices.push_back(vertex);
		}

		// build convex shape

		btCollisionShape* convexShape = new btConvexHullShape(
				&(vertices[0].getX()),vertices.size(),sizeof(btVector3));
		m_convexShapes.push_back(convexShape);

		convexShape->setMargin(m_compoundShape->getMargin());

		if(m_transformSubShapes)
		{
			btTransform trans;
			trans.setIdentity();
			trans.setOrigin(centroid);

			// add convex shape

			m_compoundShape->addChildShape(trans,convexShape);
		}
		else
		{
			btTransform trans;
			trans.setIdentity();
			//trans.setOrigin(centroid);

			// add convex shape

			m_compoundShape->addChildShape(trans,convexShape);

			//m_compoundShape->addChildShape(convexShape);
		}
	}

			virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
			{

				btTriangleMesh* trimesh = new btTriangleMesh();
				m_convexDemo->m_trimeshes.push_back(trimesh);

				btVector3 localScaling(6.f,6.f,6.f);

				//export data to .obj
				printf("ConvexResult. ");
				if (mOutputFile)
				{
					fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );

					fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
					fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);

					for (unsigned int i=0; i<result.mHullVcount; i++)
					{
						const float *p = &result.mHullVertices[i*3];
						fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
					}

					//calc centroid, to shift vertices around center of mass
					centroid.setValue(0,0,0);

					btAlignedObjectArray<btVector3> vertices;
					if ( 1 )
					{
						//const unsigned int *src = result.mHullIndices;
						for (unsigned int i=0; i<result.mHullVcount; i++)
						{
							btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
							vertex *= localScaling;
							centroid += vertex;
							
						}
					}

					centroid *= 1.f/(float(result.mHullVcount) );

					if ( 1 )
					{
						//const unsigned int *src = result.mHullIndices;
						for (unsigned int i=0; i<result.mHullVcount; i++)
						{
							btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
							vertex *= localScaling;
							vertex -= centroid ;
							vertices.push_back(vertex);
						}
					}
					
			

					if ( 1 )
					{
						const unsigned int *src = result.mHullIndices;
						for (unsigned int i=0; i<result.mHullTcount; i++)
						{
							unsigned int index0 = *src++;
							unsigned int index1 = *src++;
							unsigned int index2 = *src++;


							btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
							btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
							btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
							vertex0 *= localScaling;
							vertex1 *= localScaling;
							vertex2 *= localScaling;
							
							vertex0 -= centroid;
							vertex1 -= centroid;
							vertex2 -= centroid;


							trimesh->addTriangle(vertex0,vertex1,vertex2);

							index0+=mBaseCount;
							index1+=mBaseCount;
							index2+=mBaseCount;
							
							fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
						}
					}

				//	float mass = 1.f;
					

//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#ifdef SHRINK_OBJECT_INWARDS

					float collisionMargin = 0.01f;
					
					btAlignedObjectArray<btVector3> planeEquations;
					btGeometryUtil::getPlaneEquationsFromVertices(vertices,planeEquations);

					btAlignedObjectArray<btVector3> shiftedPlaneEquations;
//.........这里部分代码省略.........

void btOpenCLSoftBodySolverSIMDAware::optimize( btAlignedObjectArray< btSoftBody * > &softBodies ,bool forceUpdate)
{
	if( forceUpdate || m_softBodySet.size() != softBodies.size() )
	{
		// Have a change in the soft body set so update, reloading all the data
		getVertexData().clear();
		getTriangleData().clear();
		getLinkData().clear();
		m_softBodySet.resize(0);
		m_anchorIndex.clear();

		int maxPiterations = 0;
		int maxViterations = 0;

		for( int softBodyIndex = 0; softBodyIndex < softBodies.size(); ++softBodyIndex )
		{
			btSoftBody *softBody = softBodies[ softBodyIndex ];
			using Vectormath::Aos::Matrix3;
			using Vectormath::Aos::Point3;

			// Create SoftBody that will store the information within the solver
			btOpenCLAcceleratedSoftBodyInterface* newSoftBody = new btOpenCLAcceleratedSoftBodyInterface( softBody );
			m_softBodySet.push_back( newSoftBody );

			m_perClothAcceleration.push_back( toVector3(softBody->getWorldInfo()->m_gravity) );
			m_perClothDampingFactor.push_back(softBody->m_cfg.kDP);
			m_perClothVelocityCorrectionCoefficient.push_back( softBody->m_cfg.kVCF );
			m_perClothLiftFactor.push_back( softBody->m_cfg.kLF );
			m_perClothDragFactor.push_back( softBody->m_cfg.kDG );
			m_perClothMediumDensity.push_back(softBody->getWorldInfo()->air_density);
			// Simple init values. Actually we'll put 0 and -1 into them at the appropriate time
			m_perClothFriction.push_back(softBody->m_cfg.kDF);
			m_perClothCollisionObjects.push_back( CollisionObjectIndices(-1, -1) );

			// Add space for new vertices and triangles in the default solver for now
			// TODO: Include space here for tearing too later
			int firstVertex = getVertexData().getNumVertices();
			int numVertices = softBody->m_nodes.size();
			// Round maxVertices to a multiple of the workgroup size so we know we're safe to run over in a given group
			// maxVertices can be increased to allow tearing, but should be used sparingly because these extra verts will always be processed
			int maxVertices = GROUP_SIZE*((numVertices+GROUP_SIZE)/GROUP_SIZE);
			// Allocate space for new vertices in all the vertex arrays
			getVertexData().createVertices( numVertices, softBodyIndex, maxVertices );


			int firstTriangle = getTriangleData().getNumTriangles();
			int numTriangles = softBody->m_faces.size();
			int maxTriangles = numTriangles;
			getTriangleData().createTriangles( maxTriangles );

			// Copy vertices from softbody into the solver
			for( int vertex = 0; vertex < numVertices; ++vertex )
			{
				Point3 multPoint(softBody->m_nodes[vertex].m_x.getX(), softBody->m_nodes[vertex].m_x.getY(), softBody->m_nodes[vertex].m_x.getZ());
				btSoftBodyVertexData::VertexDescription desc;

				// TODO: Position in the softbody might be pre-transformed
				// or we may need to adapt for the pose.
				//desc.setPosition( cloth.getMeshTransform()*multPoint );
				desc.setPosition( multPoint );

				float vertexInverseMass = softBody->m_nodes[vertex].m_im;
				desc.setInverseMass(vertexInverseMass);
				getVertexData().setVertexAt( desc, firstVertex + vertex );

				m_anchorIndex.push_back(-1);
			}
			for( int vertex = numVertices; vertex < maxVertices; ++vertex )
			{
				m_anchorIndex.push_back(-1.0);
			}

			// Copy triangles similarly
			// We're assuming here that vertex indices are based on the firstVertex rather than the entire scene
			for( int triangle = 0; triangle < numTriangles; ++triangle )
			{
				// Note that large array storage is relative to the array not to the cloth
				// So we need to add firstVertex to each value
				int vertexIndex0 = (softBody->m_faces[triangle].m_n[0] - &(softBody->m_nodes[0]));
				int vertexIndex1 = (softBody->m_faces[triangle].m_n[1] - &(softBody->m_nodes[0]));
				int vertexIndex2 = (softBody->m_faces[triangle].m_n[2] - &(softBody->m_nodes[0]));
				btSoftBodyTriangleData::TriangleDescription newTriangle(vertexIndex0 + firstVertex, vertexIndex1 + firstVertex, vertexIndex2 + firstVertex);
				getTriangleData().setTriangleAt( newTriangle, firstTriangle + triangle );
				
				// Increase vertex triangle counts for this triangle		
				getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex0)++;
				getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex1)++;
				getVertexData().getTriangleCount(newTriangle.getVertexSet().vertex2)++;
			}

			int firstLink = getLinkData().getNumLinks();
			int numLinks = softBody->m_links.size();
			int maxLinks = numLinks;
			
			// Allocate space for the links
			getLinkData().createLinks( numLinks );

			// Add the links
			for( int link = 0; link < numLinks; ++link )
			{
//.........这里部分代码省略.........

static void generateBatchesOfWavefronts( btAlignedObjectArray < btAlignedObjectArray <int> > &linksForWavefronts, btSoftBodyLinkData &linkData, int numVertices, btAlignedObjectArray < btAlignedObjectArray <int> > &wavefrontBatches )
{
	// A per-batch map of truth values stating whether a given vertex is in that batch
	// This allows us to significantly optimize the batching
	btAlignedObjectArray <btAlignedObjectArray<bool> > mapOfVerticesInBatches;

	for( int waveIndex = 0; waveIndex < linksForWavefronts.size(); ++waveIndex )
	{
		btAlignedObjectArray <int> &wavefront( linksForWavefronts[waveIndex] );

		int batch = 0;
		bool placed = false;
		while( batch < wavefrontBatches.size() && !placed )
		{
			// Test the current batch, see if this wave shares any vertex with the waves in the batch
			bool foundSharedVertex = false;
			for( int link = 0; link < wavefront.size(); ++link )
			{
				btSoftBodyLinkData::LinkNodePair vertices = linkData.getVertexPair( wavefront[link] );
				if( (mapOfVerticesInBatches[batch])[vertices.vertex0] || (mapOfVerticesInBatches[batch])[vertices.vertex1] )
				{
					foundSharedVertex = true;
				}
			}

			if( !foundSharedVertex )
			{
				wavefrontBatches[batch].push_back( waveIndex );	
				// Insert vertices into this batch too
				for( int link = 0; link < wavefront.size(); ++link )
				{
					btSoftBodyLinkData::LinkNodePair vertices = linkData.getVertexPair( wavefront[link] );
					(mapOfVerticesInBatches[batch])[vertices.vertex0] = true;
					(mapOfVerticesInBatches[batch])[vertices.vertex1] = true;
				}
				placed = true;
			}
			batch++;
		}
		if( batch == wavefrontBatches.size() && !placed )
		{
			wavefrontBatches.resize( batch + 1 );
			wavefrontBatches[batch].push_back( waveIndex );

			// And resize map as well
			mapOfVerticesInBatches.resize( batch + 1 );
			
			// Resize maps with total number of vertices
			mapOfVerticesInBatches[batch].resize( numVertices+1, false );

			// Insert vertices into this batch too
			for( int link = 0; link < wavefront.size(); ++link )
			{
				btSoftBodyLinkData::LinkNodePair vertices = linkData.getVertexPair( wavefront[link] );
				(mapOfVerticesInBatches[batch])[vertices.vertex0] = true;
				(mapOfVerticesInBatches[batch])[vertices.vertex1] = true;
			}
		}
	}
	mapOfVerticesInBatches.clear();
}

static void computeBatchingIntoWavefronts( 
	btSoftBodyLinkData &linkData, 
	int wavefrontSize, 
	int linksPerWorkItem, 
	int maxLinksPerWavefront, 
	btAlignedObjectArray < btAlignedObjectArray <int> > &linksForWavefronts, 
	btAlignedObjectArray< btAlignedObjectArray < btAlignedObjectArray <int> > > &batchesWithinWaves, /* wave, batch, links in batch */
	btAlignedObjectArray< btAlignedObjectArray< int > > &verticesForWavefronts /* wavefront, vertex */
	)
{
	

	// Attempt generation of larger batches of links.
	btAlignedObjectArray< bool > processedLink;
	processedLink.resize( linkData.getNumLinks() );
	btAlignedObjectArray< int > listOfLinksPerVertex;
	int maxLinksPerVertex = 0;

	// Count num vertices
	int numVertices = 0;
	for( int linkIndex = 0; linkIndex < linkData.getNumLinks(); ++linkIndex )
	{
		btSoftBodyLinkData::LinkNodePair nodes( linkData.getVertexPair(linkIndex) );
		numVertices = btMax( numVertices, nodes.vertex0 + 1 );
		numVertices = btMax( numVertices, nodes.vertex1 + 1 );
	}

	// Need list of links per vertex
	// Compute valence of each vertex
	btAlignedObjectArray <int> numLinksPerVertex;
	numLinksPerVertex.resize(0);
	numLinksPerVertex.resize( numVertices, 0 );

	generateLinksPerVertex( numVertices, linkData, listOfLinksPerVertex, numLinksPerVertex, maxLinksPerVertex );

	if (!numVertices)
		return;

	for( int vertex = 0; vertex < 10; ++vertex )
	{
		for( int link = 0; link < numLinksPerVertex[vertex]; ++link )
		{
			int linkAddress = vertex * maxLinksPerVertex + link;
		}
	}


	// At this point we know what links we have for each vertex so we can start batching
	
	// We want a vertex to start with, let's go with 0
	int currentVertex = 0;
	int linksProcessed = 0;

	btAlignedObjectArray <int> verticesToProcess;

	while( linksProcessed < linkData.getNumLinks() )
	{
		// Next wavefront
		int nextWavefront = linksForWavefronts.size();
		linksForWavefronts.resize( nextWavefront + 1 );
		btAlignedObjectArray <int> &linksForWavefront(linksForWavefronts[nextWavefront]);
		verticesForWavefronts.resize( nextWavefront + 1 );
		btAlignedObjectArray<int> &vertexSet( verticesForWavefronts[nextWavefront] );

		linksForWavefront.resize(0);

		// Loop to find enough links to fill the wavefront
		// Stopping if we either run out of links, or fill it
		while( linksProcessed < linkData.getNumLinks() && linksForWavefront.size() < maxLinksPerWavefront )
		{
			// Go through the links for the current vertex
			for( int link = 0; link < numLinksPerVertex[currentVertex] && linksForWavefront.size() < maxLinksPerWavefront; ++link )
			{
				int linkAddress = currentVertex * maxLinksPerVertex + link;
				int linkIndex = listOfLinksPerVertex[linkAddress];
				
				// If we have not already processed this link, add it to the wavefront
				// Claim it as another processed link
				// Add the vertex at the far end to the list of vertices to process.
				if( !processedLink[linkIndex] )
				{
					linksForWavefront.push_back( linkIndex );
					linksProcessed++;
					processedLink[linkIndex] = true;
					int v0 = linkData.getVertexPair(linkIndex).vertex0;
					int v1 = linkData.get