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