bool CAR::LoadPhysics(
const PTree & cfg,
const std::string & carpath,
const MATHVECTOR <float, 3> & initial_position,
const QUATERNION <float> & initial_orientation,
const bool defaultabs,
const bool defaulttcs,
const bool damage,
ContentManager & content,
DynamicsWorld & world,
std::ostream & error_output)
{
std::string carmodel;
std::tr1::shared_ptr<MODEL> modelptr;
if (!cfg.get("body.mesh", carmodel, error_output)) return false;
if (!content.load(carpath, carmodel, modelptr)) return false;
btVector3 size = ToBulletVector(modelptr->GetSize());
btVector3 center = ToBulletVector(modelptr->GetCenter());
btVector3 position = ToBulletVector(initial_position);
btQuaternion rotation = ToBulletQuaternion(initial_orientation);
if (!dynamics.Load(cfg, size, center, position, rotation, damage, world, error_output)) return false;
dynamics.SetABS(defaultabs);
dynamics.SetTCS(defaulttcs);
mz_nominalmax = (GetTireMaxMz(FRONT_LEFT) + GetTireMaxMz(FRONT_RIGHT)) * 0.5;
return true;
}
void BoxPhysicsComponent::InitializeBulletComponent(BulletComponentImplementation * component)
{
component->shape = new btBoxShape(ToBulletVector(m_HalfExtentVector));
btDefaultMotionState * motionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), ToBulletVector(GetWorldPosition())));
F32 mass = 1.0f;
btVector3 inertia;
component->shape->calculateLocalInertia(mass, inertia);
btRigidBody::btRigidBodyConstructionInfo ci(mass, motionState, component->shape, inertia);
component->rigidBody = new btRigidBody(ci);
BulletPhysicsComponent::InitializeBulletComponent(component);
}
float AI_Car_Experimental::RayCastDistance( MATHVECTOR <float, 3> direction, float max_length){
btVector3 pos = car->GetCarDynamics().GetPosition();
btVector3 dir = car->GetCarDynamics().LocalToWorld(ToBulletVector(direction));
dir -= pos;
COLLISION_CONTACT contact;
car->GetDynamicsWorld()->castRay(
pos,
dir,
max_length,
&car->GetCarDynamics().getCollisionObject(),
contact
);
float depth = contact.GetDepth();
float dist = std::min(max_length, depth);
#ifdef VISUALIZE_AI_DEBUG
MATHVECTOR<float, 3> pos_start(ToMathVector<float>(pos));
MATHVECTOR<float, 3> pos_end = pos_start + (ToMathVector<float>(dir) * dist);
AddLinePoint(raycastshape, pos_start);
AddLinePoint(raycastshape, pos_end);
#endif
return dist;
}
void CAR::SetPosition(const MATHVECTOR <float, 3> & new_position)
{
btVector3 newpos = ToBulletVector(new_position);
dynamics.SetPosition(newpos);
dynamics.AlignWithGround();
}
// Ray
//-------------------------------------------------------------------------------------------------------------------------------
bool COLLISION_WORLD::CastRay(
const MATHVECTOR <float, 3> & origin,
const MATHVECTOR <float, 3> & direction,
const float length,
const btCollisionObject * caster,
COLLISION_CONTACT & contact,
int* pOnRoad) const
{
btVector3 from = ToBulletVector(origin);
btVector3 to = ToBulletVector(origin + direction * length);
MyRayResultCallback rayCallback(from, to, caster);
MATHVECTOR <float, 3> p, n; float d;
const TRACKSURFACE * s = TRACKSURFACE::None();
const BEZIER * b = NULL;
btCollisionObject * c = NULL;
// track geometry collision
world->rayTest(from, to, rayCallback);
bool geometryHit = rayCallback.hasHit();
if (geometryHit)
{
p = ToMathVector<float>(rayCallback.m_hitPointWorld);
n = ToMathVector<float>(rayCallback.m_hitNormalWorld);
d = rayCallback.m_closestHitFraction * length;
c = rayCallback.m_collisionObject;
if (c->isStaticObject() /*&& (c->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE == 0)*/)
{
void * ptr = c->getCollisionShape()->getUserPointer();
if (ptr == (void*)7777) // road
{
s = trackSurface[0];
*pOnRoad = 1;
}
if (ptr == (void*)7788) // pipe
{
s = trackSurface[0];
*pOnRoad = 2;
}
else if (ptr == 0)
{
*pOnRoad = 0;
void * ptr = c->getUserPointer();
if (ptr != NULL)
{
const TRACK_OBJECT * const obj = reinterpret_cast <const TRACK_OBJECT * const> (ptr);
assert(obj);
s = obj->GetSurface();
}
else //track geometry
{
int shapeId = rayCallback.m_shapeId;
//assert(shapeId >= 0 && shapeId < trackSurface.size());
if (shapeId >= trackSurface.size() || shapeId < 0) shapeId = 0; //crash hf-
//if (trackSurface.size() > 0)
s = trackSurface[shapeId];
}
}
}
// track bezierpatch collision
if (track != NULL)
{
MATHVECTOR <float, 3> bezierspace_raystart(origin[1], origin[2], origin[0]);
MATHVECTOR <float, 3> bezierspace_dir(direction[1], direction[2], direction[0]);
MATHVECTOR <float, 3> colpoint, colnormal;
const BEZIER * colpatch = NULL;
bool bezierHit = track->CastRay(bezierspace_raystart, bezierspace_dir, length, colpoint, colpatch, colnormal);
if (bezierHit)
{
p = MATHVECTOR <float, 3> (colpoint[2], colpoint[0], colpoint[1]);
n = MATHVECTOR <float, 3> (colnormal[2], colnormal[0], colnormal[1]);
d = (colpoint - bezierspace_raystart).Magnitude();
s = track->GetRoadSurface();
b = colpatch;
c = NULL;
*pOnRoad = 1;
}
}
contact.Set(p, n, d, s, b, c);
return true;
}
// should only happen on vehicle rollover
contact.Set(origin + direction * length, -direction, length, s, b, c);
return false;
}
bool DynamicsWorld::castRay(
const btVector3 & origin,
const btVector3 & direction,
const btScalar length,
const btCollisionObject * caster,
CollisionContact & contact) const
{
btVector3 p = origin + direction * length;
btVector3 n = -direction;
btScalar d = length;
int patch_id = -1;
const Bezier * b = 0;
const TrackSurface * s = TrackSurface::None();
const btCollisionObject * c = 0;
MyRayResultCallback ray(origin, p, caster);
rayTest(origin, p, ray);
// track geometry collision
if (ray.hasHit())
{
p = ray.m_hitPointWorld;
n = ray.m_hitNormalWorld;
d = ray.m_closestHitFraction * length;
c = ray.m_collisionObject;
if (c->isStaticObject())
{
TrackSurface * ts = static_cast<TrackSurface*>(c->getUserPointer());
if (c->getCollisionShape()->isCompound())
ts = static_cast<TrackSurface*>(ray.m_shape->getUserPointer());
// verify surface pointer
if (track)
{
const std::vector<TrackSurface> & surfaces = track->GetSurfaces();
if (ts < &surfaces[0] || ts > &surfaces[surfaces.size() - 1])
ts = NULL;
assert(ts);
}
if (ts)
s = ts;
}
// track bezierpatch collision
if (track)
{
Vec3 org = ToMathVector<float>(origin);
Vec3 dir = ToMathVector<float>(direction);
Vec3 colpoint;
Vec3 colnormal;
patch_id = contact.GetPatchId();
if (track->CastRay(org, dir, length, patch_id, colpoint, b, colnormal))
{
p = ToBulletVector(colpoint);
n = ToBulletVector(colnormal);
d = (colpoint - org).Magnitude();
}
}
contact = CollisionContact(p, n, d, patch_id, b, s, c);
return true;
}
// should only happen on vehicle rollover
contact = CollisionContact(p, n, d, patch_id, b, s, c);
return false;
}
void CARDYNAMICS::SetPosition(const MATHVECTOR<Dbl,3> & position)
{
body.SetPosition(position);
if (chassis)
chassis->translate(ToBulletVector(position) - chassis->getCenterOfMassPosition());
}
bool DynamicsWorld::castRay(
const btVector3 & origin,
const btVector3 & direction,
const btScalar length,
const btCollisionObject * caster,
COLLISION_CONTACT & contact) const
{
btVector3 p = origin + direction * length;
btVector3 n = -direction;
btScalar d = length;
int patch_id = -1;
const BEZIER * b = 0;
const TRACKSURFACE * s = TRACKSURFACE::None();
btCollisionObject * c = 0;
MyRayResultCallback ray(origin, p, caster);
rayTest(origin, p, ray);
// track geometry collision
bool geometryHit = ray.hasHit();
if (geometryHit)
{
p = ray.m_hitPointWorld;
n = ray.m_hitNormalWorld;
d = ray.m_closestHitFraction * length;
c = ray.m_collisionObject;
if (c->isStaticObject())
{
TRACKSURFACE* tsc = static_cast<TRACKSURFACE*>(c->getUserPointer());
const std::vector<TRACKSURFACE> & surfaces = track->GetSurfaces();
if (tsc >= &surfaces[0] && tsc <= &surfaces[surfaces.size()-1])
{
s = tsc;
}
#ifndef EXTBULLET
else if (c->getCollisionShape()->isCompound())
{
TRACKSURFACE* tss = static_cast<TRACKSURFACE*>(ray.m_shape->getUserPointer());
if (tss >= &surfaces[0] && tss <= &surfaces[surfaces.size()-1])
{
s = tss;
}
}
#endif
//std::cerr << "static object without surface" << std::endl;
}
// track bezierpatch collision
if (track)
{
MATHVECTOR<float, 3> org(ToMathVector<float>(origin));
MATHVECTOR<float, 3> dir(ToMathVector<float>(direction));
MATHVECTOR<float, 3> colpoint;
MATHVECTOR<float, 3> colnormal;
patch_id = contact.GetPatchId();
if (track->CastRay(org, dir, length, patch_id, colpoint, b, colnormal))
{
p = ToBulletVector(colpoint);
n = ToBulletVector(colnormal);
d = (colpoint - org).Magnitude();
}
}
contact = COLLISION_CONTACT(p, n, d, patch_id, b, s, c);
return true;
}
// should only happen on vehicle rollover
contact = COLLISION_CONTACT(p, n, d, patch_id, b, s, c);
return false;
}
//--------------------------------------------------------------------------------------------------------------------------------
// Ray
///-------------------------------------------------------------------------------------------------------------------------------
bool COLLISION_WORLD::CastRay(
const MATHVECTOR<float,3> & origin,
const MATHVECTOR<float,3> & direction, const float length,
const btCollisionObject * caster,
COLLISION_CONTACT & contact, //out
CARDYNAMICS* cd, int w, //out pCarDyn, nWheel
bool ignoreCars, bool camTilt, bool camDist) const
{
btVector3 from = ToBulletVector(origin);
btVector3 to = ToBulletVector(origin + direction * length);
MyRayResultCallback res(from, to, caster, ignoreCars, camTilt, camDist);
// data to set
MATHVECTOR<float,3> pos, norm; float dist;
const TRACKSURFACE * surf = TRACKSURFACE::None();
btCollisionObject * col = NULL; const BEZIER * bzr = NULL;
world->rayTest(from, to, res);
bool geometryHit = res.hasHit();
if (geometryHit)
{
pos = ToMathVector<float>(res.m_hitPointWorld);
norm = ToMathVector<float>(res.m_hitNormalWorld);
dist = res.m_closestHitFraction * length;
col = res.m_collisionObject;
const TerData& td = pApp->scn->sc->td;
if (col->isStaticObject() /*&& (c->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE == 0)*/)
{
long long ptrU = (long long)col->getCollisionShape()->getUserPointer(),
su = ptrU & 0xFF00, mtr = ptrU & 0xFF; //void*
/// set surface, basing on shape type -----------------
if (ptrU)
switch (su)
{
case SU_Road: // road
{
int id = td.layerRoad.surfId; // Road[mtr].
surf = &pApp->pGame->surfaces[id];
if (cd)
{ cd->iWhOnRoad[w] = 1; cd->whRoadMtr[w] = mtr; cd->whTerMtr[w] = 0; }
} break;
case SU_Pipe: // pipe
{
int id = td.layerRoad.surfId;
surf = &pApp->pGame->surfaces[id];
if (cd)
{ cd->iWhOnRoad[w] = 2; cd->whRoadMtr[w] = mtr+30; cd->whTerMtr[w] = 0; }
} break;
case SU_Terrain: // Terrain get surface from blendmap mtr
{
int t = pApp->blendMapSize;
float tws = td.fTerWorldSize;
int mx = (pos[0] + 0.5*tws)/tws*t; mx = std::max(0,std::min(t-1, mx));
int my = (pos[1] + 0.5*tws)/tws*t; my = std::max(0,std::min(t-1, t-1-my));
int mtr = pApp->blendMtr[my*t + mx];
int id = td.layersAll[td.layers[mtr]].surfId;
surf = &pApp->pGame->surfaces[id];
if (cd) // mtr 0 = not on terrain
{ cd->iWhOnRoad[w] = 0; cd->whRoadMtr[w] = 60; cd->whTerMtr[w] = mtr + 1; }
} break;
//case SU_RoadWall: //case SU_RoadColumn:
//case SU_Vegetation: case SU_Border:
//case SU_ObjectStatic: //case SU_ObjectDynamic:
default:
{
int id = td.layerRoad.surfId;
surf = &pApp->pGame->surfaces[id];
if (cd)
{ cd->iWhOnRoad[w] = 0; cd->whRoadMtr[w] = 80; cd->whTerMtr[w] = 0; }
} break;
}
else //if (ptrU == 0)
{
int id = td.layersAll[0].surfId; //0 only 1st
surf = &pApp->pGame->surfaces[id];
if (cd)
{ cd->iWhOnRoad[w] = 0; cd->whRoadMtr[w] = 0; cd->whTerMtr[w] = 1; }
/*void * ptr = col->getUserPointer();
if (ptr != NULL)
{
const TRACK_OBJECT * const obj = reinterpret_cast <const TRACK_OBJECT * const> (ptr);
assert(obj);
surf = obj->GetSurface();
}
else // track geometry
{
int shapeId = res.m_shapeId;
//assert(shapeId >= 0 && shapeId < trackSurface.size());
if (shapeId >= trackSurface.size() || shapeId < 0) shapeId = 0; //crash hf-
if (trackSurface.size() > 0)
surf = 0;//trackSurface[shapeId];
}*/
}
}
// track bezierpatch collision
if (track != NULL)
{
MATHVECTOR<float,3> bs_pos(origin[1], origin[2], origin[0]); //bezierspace
MATHVECTOR<float,3> bs_dir(direction[1], direction[2], direction[0]);
MATHVECTOR<float,3> colpos, colnorm;
const BEZIER * colpatch = NULL;
bool bezierHit = track->CastRay(bs_pos, bs_dir, length, colpos, colpatch, colnorm);
if (bezierHit)
{
pos = MATHVECTOR<float,3> (colpos[2], colpos[0], colpos[1]);
norm = MATHVECTOR<float,3> (colnorm[2], colnorm[0], colnorm[1]);
dist = (colpos - bs_pos).Magnitude();
//surf = 0;//track->GetRoadSurface();
bzr = colpatch; col = NULL;
int id = td.layerRoad.surfId;
surf = &pApp->pGame->surfaces[id];
if (cd)
{ cd->iWhOnRoad[w] = 1; cd->whRoadMtr[w] = 0; cd->whTerMtr[w] = 0; }
}
}
contact.Set(pos, norm, dist, surf, bzr, col);
return true;
}
// should only happen on vehicle rollover
contact.Set(origin + direction * length, -direction, length, surf, bzr, col);
return false;
}
void AiCarStandard::UpdateSteer()
{
#ifdef VISUALIZE_AI_DEBUG
steerlook.clear();
#endif
const Bezier *curr_patch_ptr = GetCurrentPatch(car);
//if car has no contact with track, just let it roll
if (!curr_patch_ptr)
{
if (!last_patch) return;
//if car is off track, steer the car towards the last patch it was on
//this should get the car back on track
else curr_patch_ptr = last_patch;
}
last_patch = curr_patch_ptr; //store the last patch car was on
Bezier curr_patch = RevisePatch(curr_patch_ptr, use_racingline);
#ifdef VISUALIZE_AI_DEBUG
steerlook.push_back(curr_patch);
#endif
// if there is no next patch (probably a non-closed track), let it roll
if (!curr_patch.GetNextPatch()) return;
Bezier next_patch = RevisePatch(curr_patch.GetNextPatch(), use_racingline);
// find the point to steer towards
float lookahead = 1.0;
float length = 0.0;
Vec3 dest_point = GetPatchFrontCenter(next_patch);
while (length < lookahead)
{
#ifdef VISUALIZE_AI_DEBUG
steerlook.push_back(next_patch);
#endif
length += GetPatchDirection(next_patch).Magnitude()*2.0;
dest_point = GetPatchFrontCenter(next_patch);
// if there is no next patch for whatever reason, stop lookahead
if (!next_patch.GetNextPatch())
{
length = lookahead;
break;
}
next_patch = RevisePatch(next_patch.GetNextPatch(), use_racingline);
// if next patch is a very sharp corner, stop lookahead
if (GetPatchRadius(next_patch) < LOOKAHEAD_MIN_RADIUS)
{
length = lookahead;
break;
}
}
btVector3 car_position = car->GetCenterOfMass();
btVector3 car_orientation = quatRotate(car->GetOrientation(), Direction::forward);
btVector3 desire_orientation = ToBulletVector(dest_point) - car_position;
//car's direction on the horizontal plane
car_orientation[2] = 0;
//desired direction on the horizontal plane
desire_orientation[2] = 0;
car_orientation.normalize();
desire_orientation.normalize();
//the angle between car's direction and unit y vector (forward direction)
double alpha = Angle(car_orientation[0], car_orientation[1]);
//the angle between desired direction and unit y vector (forward direction)
double beta = Angle(desire_orientation[0], desire_orientation[1]);
//calculate steering angle and direction
double angle = beta - alpha;
//angle += steerAwayFromOthers(c, dt, othercars, angle); //sum in traffic avoidance bias
if (angle > -360.0 && angle <= -180.0)
angle = -(360.0 + angle);
else if (angle > -180.0 && angle <= 0.0)
angle = - angle;
else if (angle > 0.0 && angle <= 180.0)
angle = - angle;
else if (angle > 180.0 && angle <= 360.0)
angle = 360.0 - angle;
float optimum_range = car->GetTire(FRONT_LEFT).getIdealSlipAngle() * SIMD_DEGS_PER_RAD;
angle = clamp(angle, -optimum_range, optimum_range);
float steer_value = angle / car->GetMaxSteeringAngle();
if (steer_value > 1.0) steer_value = 1.0;
else if (steer_value < -1.0) steer_value = -1.0;
assert(!std::isnan(steer_value));
inputs[CarInput::STEER_RIGHT] = steer_value;
}
