Exemplo n.º 1
0
int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
													float pivotX,float pivotY,float pivotZ,
													float axisX,float axisY,float axisZ)
{


	CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
	CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;

	RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
	RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;

	ASSERT(rb0);

	SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
	SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
	SimdVector3 axisInA(axisX,axisY,axisZ);
	SimdVector3 axisInB = rb1 ? 
		(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
	rb0->getCenterOfMassTransform().getBasis() * axisInA;

	bool angularOnly = false;

	switch (type)
	{
	case PHY_POINT2POINT_CONSTRAINT:
		{

			Point2PointConstraint* p2p = 0;

			if (rb1)
			{
				p2p = new Point2PointConstraint(*rb0,
					*rb1,pivotInA,pivotInB);
			} else
			{
				p2p = new Point2PointConstraint(*rb0,
					pivotInA);
			}

			m_constraints.push_back(p2p);
			p2p->SetUserConstraintId(gConstraintUid++);
			p2p->SetUserConstraintType(type);
			//64 bit systems can't cast pointer to int. could use size_t instead.
			return p2p->GetUserConstraintId();

			break;
		}

	case PHY_GENERIC_6DOF_CONSTRAINT:
		{
			Generic6DofConstraint* genericConstraint = 0;

			if (rb1)
			{
				SimdTransform frameInA;
				SimdTransform frameInB;
				
				SimdVector3 axis1, axis2;
				SimdPlaneSpace1( axisInA, axis1, axis2 );

				frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
					                          axisInA.y(), axis1.y(), axis2.y(),
											  axisInA.z(), axis1.z(), axis2.z() );

	
				SimdPlaneSpace1( axisInB, axis1, axis2 );
				frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
					                          axisInB.y(), axis1.y(), axis2.y(),
											  axisInB.z(), axis1.z(), axis2.z() );

				frameInA.setOrigin( pivotInA );
				frameInB.setOrigin( pivotInB );

				genericConstraint = new Generic6DofConstraint(
					*rb0,*rb1,
					frameInA,frameInB);


			} else
			{
				// TODO: Implement single body case...

			}
			

			m_constraints.push_back(genericConstraint);
			genericConstraint->SetUserConstraintId(gConstraintUid++);
			genericConstraint->SetUserConstraintType(type);
			//64 bit systems can't cast pointer to int. could use size_t instead.
			return genericConstraint->GetUserConstraintId();

			break;
		}
	case PHY_ANGULAR_CONSTRAINT:
		angularOnly = true;


	case PHY_LINEHINGE_CONSTRAINT:
		{
			HingeConstraint* hinge = 0;

			if (rb1)
			{
				hinge = new HingeConstraint(
					*rb0,
					*rb1,pivotInA,pivotInB,axisInA,axisInB);


			} else
			{
				hinge = new HingeConstraint(*rb0,
					pivotInA,axisInA);

			}
			hinge->setAngularOnly(angularOnly);

			m_constraints.push_back(hinge);
			hinge->SetUserConstraintId(gConstraintUid++);
			hinge->SetUserConstraintType(type);
			//64 bit systems can't cast pointer to int. could use size_t instead.
			return hinge->GetUserConstraintId();
			break;
		}
#ifdef NEW_BULLET_VEHICLE_SUPPORT

	case PHY_VEHICLE_CONSTRAINT:
		{
			RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
			RigidBody* chassis = rb0;
			DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
			RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
			WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
			m_wrapperVehicles.push_back(wrapperVehicle);
			vehicle->SetUserConstraintId(gConstraintUid++);
			vehicle->SetUserConstraintType(type);
			return vehicle->GetUserConstraintId();

			break;
		};
#endif //NEW_BULLET_VEHICLE_SUPPORT

	default:
		{
		}
	};

	//RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB

	return 0;

}
Exemplo n.º 2
0
int main(int argc,char** argv)
{

	int i;
	for (i=0;i<numObjects;i++)
	{
		if (i>0)
		{
			shapePtr[i] = prebuildShapePtr[1];
			shapeIndex[i] = 1;//sphere
		}
		else
		{
			shapeIndex[i] = 0;
			shapePtr[i] = prebuildShapePtr[0];
		}
	}
	
	ConvexDecomposition::WavefrontObj wo;
	char* filename = "file.obj";
	tcount = wo.loadObj(filename);

	class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
	{
		public:

		MyConvexDecomposition (FILE* outputFile)
			:mBaseCount(0),
			mHullCount(0),
			mOutputFile(outputFile)

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

				TriangleMesh* trimesh = new TriangleMesh();

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

				//export data to .obj
				printf("ConvexResult\n");
				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
					centroids[numObjects] = SimdVector3(0,0,0);
					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++;
							SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
							SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
							SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
							vertex0 *= localScaling;
							vertex1 *= localScaling;
							vertex2 *= localScaling;
							centroids[numObjects] += vertex0;
							centroids[numObjects]+= vertex1;
							centroids[numObjects]+= vertex2;
							
						}
					}

					centroids[numObjects] *= 1.f/(float(result.mHullTcount) * 3);

					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++;


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

							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 );
						}
					}

					shapeIndex[numObjects] = numObjects;
					shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh);
					
					mBaseCount+=result.mHullVcount; // advance the 'base index' counter.


				}
			}

			int   	mBaseCount;
  			int		mHullCount;
			FILE*	mOutputFile;

	};

	if (tcount)
	{
		numObjects = 1; //always have the ground object first
		
		TriangleMesh* trimesh = new TriangleMesh();

		SimdVector3 localScaling(6.f,6.f,6.f);
		
		for (int i=0;i<wo.mTriCount;i++)
		{
			int index0 = wo.mIndices[i*3];
			int index1 = wo.mIndices[i*3+1];
			int index2 = wo.mIndices[i*3+2];

			SimdVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
			SimdVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
			SimdVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
			
			vertex0 *= localScaling;
			vertex1 *= localScaling;
			vertex2 *= localScaling;

			trimesh->AddTriangle(vertex0,vertex1,vertex2);
		}

		shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh);
	}
			

	if (tcount)
	{

		char outputFileName[512];
  		strcpy(outputFileName,filename);
  		char *dot = strstr(outputFileName,".");
  		if ( dot ) 
			*dot = 0;
		strcat(outputFileName,"_convex.obj");
  		FILE* outputFile = fopen(outputFileName,"wb");
				
		unsigned int depth = 7;
		float cpercent     = 5;
		float ppercent     = 15;
		unsigned int maxv  = 16;
		float skinWidth    = 0.01;

		printf("WavefrontObj num triangles read %i",tcount);
		ConvexDecomposition::DecompDesc desc;
		desc.mVcount       =	wo.mVertexCount;
		desc.mVertices     = wo.mVertices;
		desc.mTcount       = wo.mTriCount;
		desc.mIndices      = (unsigned int *)wo.mIndices;
		desc.mDepth        = depth;
		desc.mCpercent     = cpercent;
		desc.mPpercent     = ppercent;
		desc.mMaxVertices  = maxv;
		desc.mSkinWidth    = skinWidth;

		MyConvexDecomposition	convexDecomposition(outputFile);
		desc.mCallback = &convexDecomposition;
		
		

		//convexDecomposition.performConvexDecomposition(desc);

		ConvexBuilder cb(desc.mCallback);
		int ret = cb.process(desc);
		
		if (outputFile)
			fclose(outputFile);


	}

	CollisionDispatcher* dispatcher = new	CollisionDispatcher();


	SimdVector3 worldAabbMin(-10000,-10000,-10000);
	SimdVector3 worldAabbMax(10000,10000,10000);

	OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax);
	//OverlappingPairCache* broadphase = new SimpleBroadphase();

	physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase);
	physicsEnvironmentPtr->setDeactivationTime(2.f);

	physicsEnvironmentPtr->setGravity(0,-10,0);
	PHY_ShapeProps shapeProps;

	shapeProps.m_do_anisotropic = false;
	shapeProps.m_do_fh = false;
	shapeProps.m_do_rot_fh = false;
	shapeProps.m_friction_scaling[0] = 1.;
	shapeProps.m_friction_scaling[1] = 1.;
	shapeProps.m_friction_scaling[2] = 1.;

	shapeProps.m_inertia = 1.f;
	shapeProps.m_lin_drag = 0.2f;
	shapeProps.m_ang_drag = 0.1f;
	shapeProps.m_mass = 10.0f;

	PHY_MaterialProps materialProps;
	materialProps.m_friction = 10.5f;
	materialProps.m_restitution = 0.0f;

	CcdConstructionInfo ccdObjectCi;
	ccdObjectCi.m_friction = 0.5f;

	ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag;
	ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag;

	SimdTransform tr;
	tr.setIdentity();



	for (i=0;i<numObjects;i++)
	{
		shapeProps.m_shape = shapePtr[shapeIndex[i]];
		shapeProps.m_shape->SetMargin(0.05f);



		bool isDyna = i>0;
		//if (i==1)
		//	isDyna=false;

		if (0)//i==1)
		{
			SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI);
			ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
		}


		if (i>0)
		{

			switch (i)
			{
			case 1:
				{
					ms[i].setWorldPosition(0,10,0);
					//for testing, rotate the ground cube so the stack has to recover a bit

					break;
				}
			case 2:
				{
					ms[i].setWorldPosition(0,8,2);
					break;
				}
			default:
				ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0);
			}

			float quatIma0,quatIma1,quatIma2,quatReal;
			SimdQuaternion quat;
			SimdVector3 axis(0,0,1);
			SimdScalar angle=0.5f;

			quat.setRotation(axis,angle);

			ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]);



		} else
		{
			ms[i].setWorldPosition(0,-10+EXTRA_HEIGHT,0);

		}

		ccdObjectCi.m_MotionState = &ms[i];
		ccdObjectCi.m_gravity = SimdVector3(0,0,0);
		ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0);
		if (!isDyna)
		{
			shapeProps.m_mass = 0.f;
			ccdObjectCi.m_mass = shapeProps.m_mass;
			ccdObjectCi.m_collisionFlags = CollisionObject::isStatic;
		}
		else
		{
			shapeProps.m_mass = 1.f;
			ccdObjectCi.m_mass = shapeProps.m_mass;
			ccdObjectCi.m_collisionFlags = 0;
		}


		SimdVector3 localInertia;
		if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE)
		{
			//take inertia from first shape
			shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
		} else
		{
			shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia);
		}
		ccdObjectCi.m_localInertiaTensor = localInertia;

		ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]];


		physObjects[i]= new CcdPhysicsController( ccdObjectCi);

		// Only do CCD if  motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS
		physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS;
		
		//Experimental: better estimation of CCD Time of Impact:
		//physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS;

		physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]);

		if (i==1)
		{
			//physObjects[i]->SetAngularVelocity(0,0,-2,true);
		}

		physicsEnvironmentPtr->setDebugDrawer(&debugDrawer);

	}


	//create a constraint
	if (createConstraint)
	{
		//physObjects[i]->SetAngularVelocity(0,0,-2,true);
		int constraintId;

		float pivotX=CUBE_HALF_EXTENTS,
			pivotY=-CUBE_HALF_EXTENTS,
			pivotZ=CUBE_HALF_EXTENTS;

		float axisX=1,axisY=0,axisZ=0;



		HingeConstraint* hinge = 0;

		SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
		SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
		SimdVector3 axisInA(0,1,0);
		SimdVector3 axisInB(0,-1,0);

		RigidBody* rb0 = physObjects[1]->GetRigidBody();
		RigidBody* rb1 = physObjects[2]->GetRigidBody();

		hinge = new HingeConstraint(
			*rb0,
			*rb1,pivotInA,pivotInB,axisInA,axisInB);

		physicsEnvironmentPtr->m_constraints.push_back(hinge);

		hinge->SetUserConstraintId(100);
		hinge->SetUserConstraintType(PHY_LINEHINGE_CONSTRAINT);

	}




	clientResetScene();

	setCameraDistance(26.f);

	return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://www.continuousphysics.com/Bullet/phpBB2/");
}