예제 #1
0
Object *InitObject(char *objfile)
{
	int nf;
	Object *obj;
		
	if((obj = LoadObject(objfile)) != NULL){
		SetConnectivity(obj);

		for(nf=0; nf < obj->nFaces; nf++)
			CalculatePlane(obj, nf);
	}

	return obj;
}
예제 #2
0
파일: EpaFace.cpp 프로젝트: andemi02/orkid
bool EpaFace::Initialize()
{
	assert( m_pHalfEdge && "Must setup half-edge first!" );

	CollectVertices( m_pVertices );
	
	const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
	const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;

	const btScalar e0Sqrd = e0.length2();
	const btScalar e1Sqrd = e1.length2();
	const btScalar e0e1   = e0.dot( e1 );

	m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;

	const btScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
	const btScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );

	m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
	m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;

	if ( IsAffinelyDependent() )
	{
		return false;
	}

	CalcClosestPoint();

#ifdef EPA_POLYHEDRON_USE_PLANES
	if ( !CalculatePlane() )
	{
		return false;
	}
#endif

	return true;
}
///////////////////
// Check Resolve a collision between 2 cars
void Car_CheckCarCollisions(CCar *pcCar1, CCar *pcCar2)
{
    float m1[16], m2[16];
    carsim_t *psCar1 = pcCar1->getCarSim();
    carsim_t *psCar2 = pcCar2->getCarSim();


    // First, do a quick check to make sure the cars are within a small 
    // distance of each other using a seperating axis check
    if( fabs(psCar1->cPos.GetX() - psCar2->cPos.GetX()) > 50 )
        return;
    if( fabs(psCar1->cPos.GetY() - psCar2->cPos.GetY()) > 50 )
        return;
    if( fabs(psCar1->cPos.GetZ() - psCar2->cPos.GetZ()) > 50 )
        return;

    // Clear it
    for(int i=0;i<16;i++) {
		m1[i] = 0;
        m2[i] = 0;
    }
    for(i=0;i<3;i++) {
		m1[i] = psCar1->X.GetIndex(i);
        m2[i] = psCar2->X.GetIndex(i);
    }
    for(i=4;i<7;i++) {
		m1[i] = psCar1->Y.GetIndex(i-4);
        m2[i] = psCar2->Y.GetIndex(i-4);
    }
    for(i=8;i<11;i++) {
		m1[i] = psCar1->Z.GetIndex(i-8);
        m2[i] = psCar2->Z.GetIndex(i-8);
    }
    for(i=12;i<15;i++) {
		m1[i] = psCar1->cPos.GetIndex(i-12);
        m2[i] = psCar2->cPos.GetIndex(i-12);
    }
	m1[15] = 1;
    m2[15] = 1;
    // Rase it a bit
	m1[14] += 2;
    m2[14] += 2;

    CollisionModel3D *col = pcCar1->getColMod();
    CollisionModel3D *col2 = pcCar2->getColMod();


   	col->setTransform( m1 );
    //col->setTransform( m2 );

	CVec relVel = psCar1->cVelocity - psCar2->cVelocity;
	CVec posNorm = psCar1->cPos - psCar2->cPos;
	VectorNormalize(&posNorm);


    // Check for collision
	if(col->collision( col2, -1, 0, m2 )) {

        float t1[9], t2[9];
        plane_t plane1, plane2;

        for(int n=0; n<col->getNumCollisions(); n++) {

            // Get collision normal
		    col->getCollidingTriangles(n,t1,t2,false);

		    plane2 = CalculatePlane(CVec(t1[0],t1[1],t1[2]),
			    						   CVec(t1[3],t1[4],t1[5]),
				    					   CVec(t1[6],t1[7],t1[8]));
            plane1 = CalculatePlane(CVec(t2[0],t2[1],t2[2]),
			    						   CVec(t2[3],t2[4],t2[5]),
				    					   CVec(t2[6],t2[7],t2[8]));

            //plane1.vNormal.SetZ( 0 );//MIN(plane1.vNormal.GetZ(), 0) );
            //plane2.vNormal.SetZ( 0 );//MIN(plane2.vNormal.GetZ(), 0) );

            // Make sure we are going into the plane
            //if(DotProduct(psCar1->cVelocity, plane1.vNormal) > 0)
				//continue;

            // Check to make sure the car position is the good side of the plane
			//if(DotProduct(psCar1->cPos,plane1.vNormal) + plane1.fDistance < 0)
				//continue;

			//float nRV = DotProduct(relVel, posNorm);
			//if(nRV > 0)
				//continue;



            break;
        }

        // Car 1
        CVec norm = plane1.vNormal;
	    float dot = DotProduct(plane1.vNormal, psCar1->cVelocity);
	    norm = norm * dot;
	    CVec vt = psCar1->cVelocity - norm;

        float coef = 0.01f;
		
	    // Ball-on-ball collision
	    CVec norm2 = norm * coef;
		psCar1->cVelocity = vt + norm2;
		norm2 = norm * (1.0f - coef);
		psCar2->cVelocity += norm2;


        // Car 2
        /*norm = plane2.vNormal;
	    dot = DotProduct(plane2.vNormal, psCar2->cVelocity);
	    norm = norm * dot;
	    vt = psCar2->cVelocity - norm;

        coef = 0.01f;
		
	    // Ball-on-ball collision
	    norm2 = norm * coef;
		psCar2->cVelocity = vt + norm2;
		norm2 = norm * (1.0f - coef);
		psCar1->cVelocity += norm2;*/
    }
}
///////////////////
// Check collisions between the car & track
void Car_CheckCollisions(carsim_t *psCar, CCar *pcCar, CModel *pcTrack)
{
	int		i;	
	float	m[16], t1[9], t2[9];
    float   p[3];

	// Clear it
	for(i=0;i<16;i++)
		m[i] = 0;

	for(i=0;i<3;i++)
		m[i] = psCar->X.GetIndex(i);
	for(i=4;i<7;i++)
		m[i] = psCar->Y.GetIndex(i-4);
	for(i=8;i<11;i++)
		m[i] = psCar->Z.GetIndex(i-8);
	for(i=12;i<15;i++)
		m[i] = psCar->cPos.GetIndex(i-12);
	m[15] = 1;

	// Raise the car up a bit
	m[14] += 2;


	psCar->bCollision = false;

	CollisionModel3D *col = pcCar->getColMod();//pcCar->getModel()->GetCDModel();

	col->setTransform(m);

	if(col->collision( pcTrack->getCDModel(),-1,500 )) {

		// Get collision point
		//carcol->getCollisionPoint(v1);
		//for(i=0;i<3;i++) cont->Pos.SetIndex(i,v1[i]);

		//cont->Pos = cont->Pos + offset;

		CVec oldVel = psCar->cVelocity;
		CVec oldAng = psCar->cAngVelocity;

		for(int n=0; n<col->getNumCollisions(); n++) {

			// Get collision normal
			col->getCollidingTriangles(n,t1,t2,false);
            col->getCollisionPoint(n,p,false);

			plane_t plane = CalculatePlane(CVec(t2[0],t2[1],t2[2]),
										   CVec(t2[3],t2[4],t2[5]),
										   CVec(t2[6],t2[7],t2[8]));            

			// Resolve the collision

			// Check to make sure the plane we're rebounding off is facing opposite the velocity
			if(DotProduct(psCar->cVelocity, plane.vNormal) > 0)
				continue;

			// Check to make sure the car position is the good side of the plane
			if(DotProduct(psCar->cPos,plane.vNormal) + plane.fDistance < 0)
				continue;
		
			float Mass = 2000;
			float coef = 0.2f;

			// If the velocity against the mesh is too low, make sure we fully bounce back so we don't slowly
			// go into the mesh
			if( DotProduct(psCar->cVelocity, plane.vNormal) > -1)
				coef = 1.0f;

			psCar->bCollision = true;
			psCar->cColNormal = plane.vNormal;            
            psCar->cColPoint = CVec(p[0], p[1], p[2]);

			// Linear velocity rebound
			float j = DotProduct(psCar->cVelocity * -(1+coef),plane.vNormal) / DotProduct(plane.vNormal,plane.vNormal * (1/Mass));
			psCar->cVelocity += plane.vNormal * (j*(1/Mass));



			// Angular velocity rebound
			float pnt[3];
			col->getCollisionPoint(n, pnt);
			//CVec p = CVec(pnt[0],pnt[1],pnt[2]);
			CVec p = psCar->X*pnt[0] + psCar->Y*pnt[1] + psCar->Z*pnt[2];
			CVec v = CrossProduct(oldAng, p) + oldVel;

			if(DotProduct(plane.vNormal,v) > EPSILON)
				continue;

			if(DotProduct(p+psCar->cPos,plane.vNormal) + plane.fDistance > EPSILON)
				continue;

			v = CrossProduct(p, plane.vNormal*-DotProduct(v,plane.vNormal) * 0.01f);
			
			//psCar->cAngVelocity += v;
		}

		//rbody.ResolveCollisions();

		//return true;
	}
}
///////////////////
// Update the wheel position
void Car_UpdateWheel(carsim_t *psCar, CModel *pcTrack, int id)
{
	wheel_t *w = &psCar->sWheels[id];

	CVec	relpos = w->cRelPos;
	float	p[3], t1[9], t2[9];
	plane_t	plane;

	// Calculate the wheel 'top' pos
	CVec wheelPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;

	// Convert the wheel in car coords
	relpos -= CVec(0,0,w->fRestLength);	
	w->cPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;	
	
	// This is for the rendering
	w->fWheelZ = -w->fRestLength;	

	// Defaults
	w->fSuspLength = w->fRestLength;
	w->fPistonVelocity = 0;


	// Check for collisions
	p[0] = w->cPos.GetX();	p[1] = w->cPos.GetY();	p[2] = w->cPos.GetZ();

	w->bCollision = false;
	float	pDist = 0;

	if( pcTrack->getCDModel()->sphereCollision( p, w->fRadius ) ) {

		float top=999999;
		for(int n=0; n<pcTrack->getCDModel()->getNumCollisions(); n++) {

			pcTrack->getCDModel()->getCollidingTriangles(n, t1,t2,false);		

			plane = CalculatePlane(CVec(t1[0],t1[1],t1[2]),
										   CVec(t1[3],t1[4],t1[5]),
										   CVec(t1[6],t1[7],t1[8]));			

			// If the normal is too great for the tyre, ignore the collision
			if(DotProduct(plane.vNormal,psCar->Z) < 0.5f)
				continue;

			// Find the plane that is raised towards the wheel center the most
			if( DotProduct(wheelPos, plane.vNormal) + plane.fDistance < top) {

				// TODO: Check if the tyre is not too high (ie, over the suspension joint)
				top = DotProduct(wheelPos, plane.vNormal) + plane.fDistance;
				w->cNormal = plane.vNormal;
				pDist = plane.fDistance;
			}
						
			w->bCollision = true;
		}
	}

	w->fSpeed = 0;

	// If not colliding, just leave
	if(!w->bCollision)
		return;


	// Make the tyre sit on top of the road
	float d = DotProduct(w->cPos, w->cNormal) + pDist;
	w->fSuspLength = d;

	relpos = w->cRelPos - CVec(0,0,w->fSuspLength);
	w->fWheelZ = -w->fSuspLength;
	w->cPos = psCar->X*relpos.GetX() + psCar->Y*relpos.GetY() + psCar->Z*relpos.GetZ() + psCar->cPos;

	// Get the piston velocity for the suspension
	CVec vel = CrossProduct(psCar->cPos-w->cPos, psCar->cAngVelocity) + psCar->cVelocity;

	w->fPistonVelocity = (psCar->Z * DotProduct(psCar->Z,vel)).GetZ();


	// Calculate the rotation speed of the tyre
	CVec s = psCar->Y * DotProduct(psCar->cVelocity, psCar->Y);
	w->fSpeed = VectorLength(s);    

	if(DotProduct(psCar->cVelocity, psCar->Y) > 0)
		w->fSpin += w->fSpeed / w->fRadius;
	else
		w->fSpin -= w->fSpeed / w->fRadius;


    // Rotation is based on the Torque from the engine
    //w->fSpin += w->fTorque / w->fRadius;

    // If the torque is greater then the slip torque for the surface, the wheel is slipping and the engine force
    // is less
    // Note: The slip torque is constant for now
    if(id == 1) {
        //tMainSR3.f1 = w->fTorque;
    }

    if(w->fTorque > 100) {
        // Slipping
        w->fEngineLoad = w->fTorque/50;
        w->bSlip = true;
    } else {
        w->bSlip = false;
        w->fEngineLoad = 0;
    }


	w->fSpin = LimitAngle(w->fSpin);
}
예제 #6
0
파일: poly.cpp 프로젝트: stefanha/map-files
void Poly::SortVerticesCW ( )
{
	//
	// Calculate center of polygon
	//
	Vector3	center;

	for ( int i = 0; i < GetNumberOfVertices ( ); i++ )
	{
		center = center + verts[ i ].p;
	}

	center = center / GetNumberOfVertices ( );

	//
	// Sort vertices
	//
	for ( i = 0; i < GetNumberOfVertices ( ) - 2; i++ )
	{
		Vector3	a;
		Plane	p;
		double	SmallestAngle	= -1;
		int		Smallest		= -1;

		a = verts[ i ].p - center;
		a.Normalize ( );

		p.PointsToPlane ( verts[ i ].p, center, center + plane.n );

		for ( int j = i + 1; j < GetNumberOfVertices ( ); j++ )
		{
			if ( p.ClassifyPoint ( verts[ j ].p ) != Plane::eCP::BACK )
			{
				Vector3	b;
				double	Angle;
				
				b = verts[ j ].p - center;
				b.Normalize ( );

				Angle = a.Dot ( b );

				if ( Angle > SmallestAngle )
				{
					SmallestAngle	= Angle;
					Smallest		= j;
				}
			}
		}

		if ( Smallest == -1 )
		{
			cout << "Error: Degenerate polygon!" << endl;

			abort ( );
		}

		Vertex	t			= verts[ Smallest ];
		verts[ Smallest ]	= verts[ i + 1 ];
		verts[ i + 1 ]		= t;
	}

	//
	// Check if vertex order needs to be reversed for back-facing polygon
	//
	Plane	oldPlane = plane;

	CalculatePlane ( );

	if ( plane.n.Dot ( oldPlane.n ) < 0 )
	{
		int j = GetNumberOfVertices ( );

		for ( int i = 0; i < j / 2; i++ )
		{
			Vertex v			= verts[ i ];
			verts[ i ]			= verts[ j - i - 1 ];
			verts[ j - i - 1 ]	= v;
		}
	}
}