C++ btAlignedObjectArray类代码示例

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

//--------------------------------------------------------------------------------------
// Create any D3D11 resources that aren't dependant on the back buffer
//--------------------------------------------------------------------------------------
HRESULT CALLBACK OnD3D11CreateDevice( ID3D11Device* pd3dDevice, const DXGI_SURFACE_DESC* pBackBufferSurfaceDesc,
                                      void* pUserContext )
{
	
    g_pd3dDevice = pd3dDevice;
 
    HRESULT hr;

    ID3D11DeviceContext* pd3dImmediateContext = DXUTGetD3D11DeviceContext();

    V_RETURN( g_DialogResourceManager.OnD3D11CreateDevice( pd3dDevice, pd3dImmediateContext ) );
    V_RETURN( g_D3DSettingsDlg.OnD3D11CreateDevice( pd3dDevice ) );
    g_pTxtHelper = new CDXUTTextHelper( pd3dDevice, pd3dImmediateContext, &g_DialogResourceManager, 15 );

    D3DXVECTOR3 vCenter( 0.25767413f, -28.503521f, 111.00689f);
    FLOAT fObjectRadius = 378.15607f;

    D3DXMatrixTranslation( &g_mCenterMesh, -vCenter.x, -vCenter.y, -vCenter.z );
    D3DXMATRIXA16 m;
    D3DXMatrixRotationY( &m, D3DX_PI );
    g_mCenterMesh *= m;
    D3DXMatrixRotationX( &m, D3DX_PI / 2.0f );
    g_mCenterMesh *= m;

    // Compile the shaders to a model based on the feature level we acquired
    ID3DBlob* pVertexShaderBuffer = NULL;
	ID3DBlob* pGeometryShaderBuffer = NULL;
    ID3DBlob* pPixelShaderBuffer = NULL;
  
    switch( DXUTGetD3D11DeviceFeatureLevel() )
    {
        case D3D_FEATURE_LEVEL_11_0:
            V_RETURN( CompileShaderFromFile( L"cloth_renderer_VS.hlsl", "VSMain", "vs_5_0" , &pVertexShaderBuffer ) );
			V_RETURN( CompileShaderFromFile( L"cloth_renderer_PS.hlsl", "GSMain", "gs_5_0" , &pGeometryShaderBuffer ) );
            V_RETURN( CompileShaderFromFile( L"cloth_renderer_PS.hlsl", "PSMain", "ps_5_0" , &pPixelShaderBuffer ) );
            break;        
    }

    // Create the shaders
    V_RETURN( pd3dDevice->CreateVertexShader( pVertexShaderBuffer->GetBufferPointer(),
                                              pVertexShaderBuffer->GetBufferSize(), NULL, &g_pVertexShader ) );


	V_RETURN( pd3dDevice->CreateGeometryShader( pGeometryShaderBuffer->GetBufferPointer(),
                                              pGeometryShaderBuffer->GetBufferSize(), NULL, &g_pGeometryShader ) );
    

    V_RETURN( pd3dDevice->CreatePixelShader( pPixelShaderBuffer->GetBufferPointer(),
                                             pPixelShaderBuffer->GetBufferSize(), NULL, &g_pPixelShader ) );

    

    V_RETURN( pd3dDevice->CreateInputLayout( layout, ARRAYSIZE( layout ), pVertexShaderBuffer->GetBufferPointer(),
                                             pVertexShaderBuffer->GetBufferSize(), &g_pVertexLayout11 ) );

    SAFE_RELEASE( pVertexShaderBuffer );
    SAFE_RELEASE( pPixelShaderBuffer );
	SAFE_RELEASE( pGeometryShaderBuffer );


    // Load the mesh
    V_RETURN( g_Mesh11.Create( pd3dDevice, L"tiny\\tiny.sdkmesh", true ) );


    

    // Create a sampler state
    D3D11_SAMPLER_DESC SamDesc;
    SamDesc.Filter = D3D11_FILTER_MIN_MAG_MIP_LINEAR;
    SamDesc.AddressU = D3D11_TEXTURE_ADDRESS_WRAP;
    SamDesc.AddressV = D3D11_TEXTURE_ADDRESS_WRAP;
    SamDesc.AddressW = D3D11_TEXTURE_ADDRESS_WRAP;
    SamDesc.MipLODBias = 0.0f;
    SamDesc.MaxAnisotropy = 1;
    SamDesc.ComparisonFunc = D3D11_COMPARISON_ALWAYS;
    SamDesc.BorderColor[0] = SamDesc.BorderColor[1] = SamDesc.BorderColor[2] = SamDesc.BorderColor[3] = 0;
    SamDesc.MinLOD = 0;
    SamDesc.MaxLOD = D3D11_FLOAT32_MAX;
    V_RETURN( pd3dDevice->CreateSamplerState( &SamDesc, &g_pSamLinear ) );


	

    // Setup constant buffers
    D3D11_BUFFER_DESC Desc;
    Desc.Usage = D3D11_USAGE_DYNAMIC;
    Desc.BindFlags = D3D11_BIND_CONSTANT_BUFFER;
    Desc.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE;
    Desc.MiscFlags = 0;

    Desc.ByteWidth = sizeof( CB_VS_PER_OBJECT );
    V_RETURN( pd3dDevice->CreateBuffer( &Desc, NULL, &g_pcbVSPerObject ) );

    Desc.ByteWidth = sizeof( CB_PS_PER_OBJECT );
    V_RETURN( pd3dDevice->CreateBuffer( &Desc, NULL, &g_pcbPSPerObject ) );

    Desc.ByteWidth = sizeof( CB_PS_PER_FRAME );
//.........这里部分代码省略.........

//--------------------------------------------------------------------------------------
// Render the scene using the D3D11 device
//--------------------------------------------------------------------------------------
void CALLBACK OnD3D11FrameRender( ID3D11Device* pd3dDevice, ID3D11DeviceContext* pd3dImmediateContext, double fTime,
                                  float fElapsedTime, void* pUserContext )
{




	//float ms = getDeltaTimeMicroseconds();
	btScalar dt = (btScalar)m_clock.getTimeMicroseconds();
	m_clock.reset();

	///step the simulation
	if (m_dynamicsWorld && !paused)
	{

		m_dynamicsWorld->stepSimulation(dt / 1000000.f);

		updatePhysicsWorld();
	}

	//paused = 1;
	


	///////////////////////////////////////////////////////

    HRESULT hr;

    // If the settings dialog is being shown, then render it instead of rendering the app's scene
    if( g_D3DSettingsDlg.IsActive() )
    {
        g_D3DSettingsDlg.OnRender( fElapsedTime );
        return;
    }

    // Clear the render target and depth stencil
    float ClearColor[4] = { 0.0f, 0.25f, 0.25f, 0.55f };
    ID3D11RenderTargetView* pRTV = DXUTGetD3D11RenderTargetView();
    pd3dImmediateContext->ClearRenderTargetView( pRTV, ClearColor );
    ID3D11DepthStencilView* pDSV = DXUTGetD3D11DepthStencilView();
    pd3dImmediateContext->ClearDepthStencilView( pDSV, D3D11_CLEAR_DEPTH, 1.0, 0 );


	for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
	{	
		g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor );
		cloths[flagIndex].draw();
	}

	my_capsule.draw();

	
    DXUT_BeginPerfEvent( DXUT_PERFEVENTCOLOR, L"HUD / Stats" );
    g_HUD.OnRender( fElapsedTime );
    g_SampleUI.OnRender( fElapsedTime );
    RenderText();
    DXUT_EndPerfEvent();


/*
	 SAFE_RELEASE(pRTV);
     SAFE_RELEASE(pDSV);
*/

 
}

void PhysicsClient::submitCommand(int command)
{
	m_userCommandRequests.push_back(command);
	b3Printf("User submitted command request  %d (outstanding %d)\n",command, m_userCommandRequests.size());
}

void	PhysicsClient::initPhysics()
{
	if (m_guiHelper && m_guiHelper->getParameterInterface())
	{
		{
			bool isTrigger = false;
			ButtonParams button("Load URDF",CMD_LOAD_URDF,  isTrigger);
			button.m_callback = MyCallback;
			button.m_userPointer = this;
			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
		}

		{
			bool isTrigger = false;
			ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
			button.m_callback = MyCallback;
			button.m_userPointer = this;
			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
		}

		{
			bool isTrigger = false;
			ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE,  isTrigger);
			button.m_callback = MyCallback;
			button.m_userPointer = this;
			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
		}
		
		{
			bool isTrigger = false;
			ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
			button.m_callback = MyCallback;
			button.m_userPointer = this;
			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
		}
		
		{
			bool isTrigger = false;
			ButtonParams button("Shut Down",CMD_SHUTDOWN,  isTrigger);
			button.m_callback = MyCallback;
			button.m_userPointer = this;
			m_guiHelper->getParameterInterface()->registerButtonParameter(button);
		}
	} else
	{
		m_userCommandRequests.push_back(CMD_LOAD_URDF);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
		//m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
		//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
		m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		m_userCommandRequests.push_back(CMD_SHUTDOWN);
		
	}


    m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
    if (m_testBlock1)
    {
     //   btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
        if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
        {
            b3Error("Error: please start server before client\n");
            m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
            m_testBlock1 = 0;
        } else
		{
			b3Printf("Shared Memory status is OK\n");
		}
    } else
	{
		m_wantsTermination = true;
	}
}