Пример #1
0
void
SimCarCollideCars(tSituation *s)
{
    tCar *car;
    tCarElt *carElt;
    int i;

    for (i = 0; i < s->_ncars; i++) {
        carElt = s->cars[i];
        if (carElt->_state & RM_CAR_STATE_NO_SIMU) {
            continue;
        }

        car = &(SimCarTable[carElt->index]);
        dtSelectObject(car);
        // Fit the bounding box around the car, statGC's are the static offsets.
        dtLoadIdentity();
        dtTranslate(-carElt->_statGC_x, -carElt->_statGC_y, 0.0f);
        // Place the bounding box such that it fits the car in the world.
        dtMultMatrixf((const float *)(carElt->_posMat));
        memset(&(car->VelColl), 0, sizeof(tPosd));
    }

    // Running the collision detection. If no collision is detected, call dtProceed.
    // dtProceed just works if all objects are disjoint.
    if (dtTest() == 0) {
        dtProceed();
    }

    for (i = 0; i < s->_ncars; i++) {
        carElt = s->cars[i];
        if (carElt->_state & RM_CAR_STATE_NO_SIMU) {
            continue;
        }
        car = &(SimCarTable[carElt->index]);
        if (car->collision & SEM_COLLISION_CAR) {
            car->DynGCg.vel.x = car->VelColl.x;
            car->DynGCg.vel.y = car->VelColl.y;
            car->rot_mom[SG_Z] = car->VelColl.az/car->Iinv.z;
            car->DynGC.vel.az = car->DynGCg.vel.az = -2.0f*car->rot_mom[SG_Z] * car->Iinv.z;
        }
    }
}
Пример #2
0
// Collision response for walls.
// TODO: Separate common code with car-car collision response.
static void SimCarWallCollideResponse(void *clientdata, DtObjectRef obj1, DtObjectRef obj2, const DtCollData *collData)
{
	tCar* car;		// The car colliding with the wall.
	float nsign;	// Normal direction correction for collision plane.
	sgVec2 p;		// Cars collision point delivered by solid.

	// TODO: If other movable objects are added which could collide with the wall, it will be
	// necessary to validate if the object is actually a car.

	if (obj1 == clientdata) {
		car = (tCar*) obj2;
		nsign = -1.0f;
		p[0] = (float) collData->point2[0];
		p[1] = (float) collData->point2[1];
	} else {
		car = (tCar*) obj1;
		nsign = 1.0f;
		p[0] = (float) collData->point1[0];
		p[1] = (float) collData->point1[1];
	}

	sgVec2 n;		// Collision normal delivered by solid, corrected such that it points away from the wall.
	n[0] = nsign * (float) collData->normal[0];
	n[1] = nsign * (float) collData->normal[1];
	float pdist = sgLengthVec2(n);	// Distance of collision points.
	sgNormaliseVec2(n);

	sgVec2 r;
	sgSubVec2(r, p, (const float*)&(car->statGC));

	tCarElt *carElt = car->carElt;

	sgVec2 vp;		// Speed of car collision point in global frame of reference.
	sgVec2 rg;		// raduis oriented in global coordinates, still relative to CG (rotated aroung CG).

	float sina = sin(carElt->_yaw);
	float cosa = cos(carElt->_yaw);
	rg[0] = r[0]*cosa - r[1]*sina;
	rg[1] = r[0]*sina + r[1]*cosa;

	vp[0] = car->DynGCg.vel.x - car->DynGCg.vel.az * rg[1];
	vp[1] = car->DynGCg.vel.y + car->DynGCg.vel.az * rg[0];

	sgVec2 tmpv;
	static const float CAR_MIN_MOVEMENT = 0.02f;
	static const float CAR_MAX_MOVEMENT = 0.05f;
	sgScaleVec2(tmpv, n, MIN(MAX(pdist, CAR_MIN_MOVEMENT), CAR_MAX_MOVEMENT));
	if (car->blocked == 0) {
		sgAddVec2((float*)&(car->DynGCg.pos), tmpv);
		car->blocked = 1;
    }

	// Doing no dammage and correction if the cars are moving out of each other.
	if (sgScalarProductVec2(vp, n) > 0) {
		return;
	}

	float rp = sgScalarProductVec2(rg, n);

	// Pesudo cross product to find out if we are left or right.
	// TODO: SIGN, scrap value?
	float rpsign = n[0]*rg[1] - n[1]*rg[0];

	const float e = 1.0f;	// energy restitution
	float j = -(1.0f + e) * sgScalarProductVec2(vp, n) / (car->Minv + rp * rp * car->Iinv.z);
	const float ROT_K = 0.5f;

	// Damage.
	tdble damFactor, atmp;
	atmp = atan2(r[1], r[0]);
	if (fabs(atmp) < (PI / 3.0)) {
		// Front collision gives more damage.
		damFactor = 1.5f;
	} else {
		// Rear collision gives less damage.
		damFactor = 1.0f;
	}

	static const float DMGFACTOR = 0.00002f;
	if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
		car->dammage += (int)(CAR_DAMMAGE * (DMGFACTOR*j*j) * damFactor * simDammageFactor[car->carElt->_skillLevel]);
	}

	sgScaleVec2(tmpv, n, j * car->Minv);
	sgVec2 v2a;

	if (car->collision & SEM_COLLISION_CAR) {
		sgAddVec2(v2a, (const float*)&(car->VelColl.x), tmpv);
		car->VelColl.az = car->VelColl.az + j * rp * rpsign * car->Iinv.z * ROT_K;
	} else {
		sgAddVec2(v2a, (const float*)&(car->DynGCg.vel), tmpv);
		car->VelColl.az = car->DynGCg.vel.az + j * rp * rpsign * car->Iinv.z * ROT_K;
	}

	static float VELMAX = 3.0f;
	if (fabs(car->VelColl.az) > VELMAX) {
		car->VelColl.az = SIGN(car->VelColl.az) * VELMAX;
	}

	sgCopyVec2((float*)&(car->VelColl.x), v2a);

	// Move the car for the collision lib.
	sgMakeCoordMat4(carElt->pub.posMat, car->DynGCg.pos.x, car->DynGCg.pos.y,
					car->DynGCg.pos.z - carElt->_statGC_z, RAD2DEG(carElt->_yaw),
					RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
	dtSelectObject(car);
	dtLoadIdentity();
	dtTranslate(-carElt->_statGC_x, -carElt->_statGC_y, 0.0f);
	dtMultMatrixf((const float *)(carElt->_posMat));

	car->collision |= SEM_COLLISION_CAR;
}
Пример #3
0
static void SimCarCollideResponse(void * /*dummy*/, DtObjectRef obj1, DtObjectRef obj2, const DtCollData *collData)
{
	sgVec2 n;		// Collision normal delivered by solid: Global(point1) - Global(point2)
	tCar *car[2];	// The cars.
	sgVec2 p[2];	// Collision points delivered by solid, in body local coordinates.
	sgVec2 r[2];	// Collision point relative to center of gravity.
	sgVec2 vp[2];	// Speed of collision point in world coordinate system.
	sgVec3 pt[2];	// Collision points in global coordinates.

	int i;

	car[0] = (tCar*)obj1;
	car[1] = (tCar*)obj2;

	// Handle cars collisions during pit stops as well.
	static const int NO_SIMU_WITHOUT_PIT = RM_CAR_STATE_NO_SIMU & ~RM_CAR_STATE_PIT;

	if ((car[0]->carElt->_state & NO_SIMU_WITHOUT_PIT) ||
		(car[1]->carElt->_state & NO_SIMU_WITHOUT_PIT))
	{
		return;
	}

    if (car[0]->carElt->index < car[1]->carElt->index) {
		// vector conversion from double to float.
		p[0][0] = (float)collData->point1[0];
		p[0][1] = (float)collData->point1[1];
		p[1][0] = (float)collData->point2[0];
		p[1][1] = (float)collData->point2[1];
		n[0]  = (float)collData->normal[0];
		n[1]  = (float)collData->normal[1];
    } else {
		// swap the cars (not the same for the simu).
		car[0] = (tCar*)obj2;
		car[1] = (tCar*)obj1;
		p[0][0] = (float)collData->point2[0];
		p[0][1] = (float)collData->point2[1];
		p[1][0] = (float)collData->point1[0];
		p[1][1] = (float)collData->point1[1];
		n[0]  = -(float)collData->normal[0];
		n[1]  = -(float)collData->normal[1];
	}

	sgNormaliseVec2(n);

	sgVec2 rg[2];	// radius oriented in global coordinates, still relative to CG (rotated aroung CG).
	tCarElt *carElt;

	for (i = 0; i < 2; i++) {
		// vector GP (Center of gravity to collision point). p1 and p2 are delivered from solid as
		// points in the car coordinate system.
		sgSubVec2(r[i], p[i], (const float*)&(car[i]->statGC));

		// Speed of collision points, linear motion of center of gravity (CG) plus rotational
		// motion around the CG.
		carElt = car[i]->carElt;
		float sina = sin(carElt->_yaw);
		float cosa = cos(carElt->_yaw);
		rg[i][0] = r[i][0]*cosa - r[i][1]*sina;
		rg[i][1] = r[i][0]*sina + r[i][1]*cosa;

		vp[i][0] = car[i]->DynGCg.vel.x - car[i]->DynGCg.vel.az * rg[i][1];
		vp[i][1] = car[i]->DynGCg.vel.y + car[i]->DynGCg.vel.az * rg[i][0];
	}

	// Relative speed of collision points.
	sgVec2 v1ab;
	sgSubVec2(v1ab, vp[0], vp[1]);

	// try to separate the cars. The computation is necessary because dtProceed is not called till
	// the collision is resolved. 
	for (i = 0; i < 2; i++) {
		sgCopyVec2(pt[i], r[i]);
		pt[i][2] = 0.0f;
		// Transform points relative to cars local coordinate system into global coordinates.
		sgFullXformPnt3(pt[i], car[i]->carElt->_posMat);
	}

	// Compute distance of collision points.
	sgVec3 pab;
	sgSubVec2(pab, pt[1], pt[0]);
	float distpab = sgLengthVec2(pab);

	sgVec2 tmpv;
	
	sgScaleVec2(tmpv, n, MIN(distpab, 0.05));
	// No "for" loop here because of subtle difference AddVec/SubVec. 
	if (car[0]->blocked == 0 && !(car[0]->carElt->_state & RM_CAR_STATE_NO_SIMU)) {
		sgAddVec2((float*)&(car[0]->DynGCg.pos), tmpv);
		car[0]->blocked = 1;
    }
	if (car[1]->blocked == 0 && !(car[1]->carElt->_state & RM_CAR_STATE_NO_SIMU)) {
		sgSubVec2((float*)&(car[1]->DynGCg.pos), tmpv);
		car[1]->blocked = 1;
    }

	// Doing no dammage and correction if the cars are moving out of each other.
	if (sgScalarProductVec2(v1ab, n) > 0) {
		return;
	}

	// impulse.
	float rpn[2];
	rpn[0] = sgScalarProductVec2(rg[0], n);
	rpn[1] = sgScalarProductVec2(rg[1], n);

	// Pseudo cross product to find out if we are left or right.
	// TODO: SIGN, scrap value?
	float rpsign[2];
	rpsign[0] =  n[0]*rg[0][1] - n[1]*rg[0][0];
	rpsign[1] = -n[0]*rg[1][1] + n[1]*rg[1][0];

	const float e = 1.0f;	// energy restitution

	float j = -(1.0f + e) * sgScalarProductVec2(v1ab, n) /
		((car[0]->Minv + car[1]->Minv) +
		rpn[0] * rpn[0] * car[0]->Iinv.z + rpn[1] * rpn[1] * car[1]->Iinv.z);

	for (i = 0; i < 2; i++) {
		if (car[i]->carElt->_state & RM_CAR_STATE_NO_SIMU) {
			continue;
		}

		// Damage.
		tdble damFactor, atmp;
		atmp = atan2(r[i][1], r[i][0]);
		if (fabs(atmp) < (PI / 3.0)) {
			// Front collision gives more damage.
			damFactor = 1.5f;
		} else {
			// Rear collision gives less damage.
			damFactor = 1.0f;
		}

		if ((car[i]->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
			car[i]->dammage += (int)(CAR_DAMMAGE * fabs(j) * damFactor * simDammageFactor[car[i]->carElt->_skillLevel]);
		}

		// Compute collision velocity.
		const float ROT_K = 1.0f;

		float js = (i == 0) ? j : -j;
		sgScaleVec2(tmpv, n, js * car[i]->Minv);
		sgVec2 v2a;

		if (car[i]->collision & SEM_COLLISION_CAR) {
			sgAddVec2(v2a, (const float*)&(car[i]->VelColl.x), tmpv);
			car[i]->VelColl.az = car[i]->VelColl.az + js * rpsign[i] * rpn[i] * car[i]->Iinv.z * ROT_K;
		} else {
			sgAddVec2(v2a, (const float*)&(car[i]->DynGCg.vel), tmpv);
			car[i]->VelColl.az = car[i]->DynGCg.vel.az + js * rpsign[i] * rpn[i] * car[i]->Iinv.z * ROT_K;
		}

		static float VELMAX = 3.0f;
		if (fabs(car[i]->VelColl.az) > VELMAX) {
			car[i]->VelColl.az = SIGN(car[i]->VelColl.az) * VELMAX;
		}

		sgCopyVec2((float*)&(car[i]->VelColl.x), v2a);

		// Move the car for the collision lib.
		tCarElt *carElt = car[i]->carElt;
		sgMakeCoordMat4(carElt->pub.posMat, car[i]->DynGCg.pos.x, car[i]->DynGCg.pos.y,
						car[i]->DynGCg.pos.z - carElt->_statGC_z, RAD2DEG(carElt->_yaw),
						RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
		dtSelectObject(car[i]);
		dtLoadIdentity();
		dtTranslate(-carElt->_statGC_x, -carElt->_statGC_y, 0.0f);
		dtMultMatrixf((const float *)(carElt->_posMat));

		car[i]->collision |= SEM_COLLISION_CAR;
	}
}