From 271a157c6cf5865a2fd3197ec3a6a0c155d8e6cb Mon Sep 17 00:00:00 2001 From: davemk Date: Tue, 18 Aug 2009 11:05:40 +0000 Subject: [PATCH] use kart projection for rescue and initial simulation git-svn-id: svn+ssh://svn.code.sf.net/p/supertuxkart/code/main/branches/irrlicht@3882 178a84e3-b1eb-0310-8ba1-8eac791a3b58 --- src/modes/linear_world.cpp | 28 ++++- src/modes/world.cpp | 16 ++- src/physics/btKart.cpp | 245 +++++++++++++++++++++++++++++++++++++ src/physics/btKart.hpp | 2 + src/physics/physics.cpp | 11 ++ src/physics/physics.hpp | 1 + 6 files changed, 298 insertions(+), 5 deletions(-) diff --git a/src/modes/linear_world.cpp b/src/modes/linear_world.cpp index 99cff45d0..07285f7c1 100644 --- a/src/modes/linear_world.cpp +++ b/src/modes/linear_world.cpp @@ -479,17 +479,37 @@ void LinearWorld::moveKartAfterRescue(Kart* kart, btRigidBody* body) m_track->getAngle(info.m_track_sector) ); kart->setRotation(heading); - // A certain epsilon is added here to the Z coordinate (0.1), in case + // A certain epsilon is added here to the Z coordinate, in case // that the drivelines are somewhat under the track. Otherwise, the - // kart will be placed a little bit under the track, triggering - // a rescue, ... + // kart might be placed a little bit under the track, triggering + // a rescue, ... (experimentally found value) + float epsilon = 0.5f * kart->getKartHeight(); + btTransform pos; - pos.setOrigin(kart->getXYZ()+btVector3(0, 0, 0.5f*kart->getKartHeight()+0.1f)); + pos.setOrigin(kart->getXYZ()+btVector3(0, 0, kart->getKartHeight() + epsilon)); pos.setRotation(btQuaternion(btVector3(0.0f, 0.0f, 1.0f), m_track->getAngle(info.m_track_sector))); body->setCenterOfMassTransform(pos); + //project kart to surface of track + bool kart_over_ground = m_physics->projectKartDownwards(kart); + + if (kart_over_ground) + { + //add vertical offset so that the kart starts off above track + + //TODO - offset needs to be a configurable parameter + //float vertical_offset = 0.5f * kart->getKartHeight(); + //body->translate(btVector3(0, 0, vertical_offset)); + } + else + { + fprintf(stderr, "WARNING: invalid position after rescue for kart %s on track %s.\n", + (kart->getIdent().c_str()), m_track->getIdent().c_str()); + } + + } // moveKartAfterRescue //----------------------------------------------------------------------------- diff --git a/src/modes/world.cpp b/src/modes/world.cpp index 7bf990fdf..cb388a094 100644 --- a/src/modes/world.cpp +++ b/src/modes/world.cpp @@ -205,13 +205,27 @@ void World::terminateRace() */ void World::resetAllKarts() { + //Project karts onto track from above. This will lower each kart so + //that at least one of its wheel will be on the surface of the track + for ( Karts::iterator i=m_kart.begin(); i!=m_kart.end(); i++) + { + bool kart_over_ground = m_physics->projectKartDownwards(*i); + + if (!kart_over_ground) + { + fprintf(stderr, "ERROR: no valid starting position for kart %d on track %s.\n", + (int)(i-m_kart.begin()), m_track->getIdent().c_str()); + exit(-1); + } + } + bool all_finished=false; // kart->isInRest() is not fully correct, since it only takes the // velocity in count, which might be close to zero when the kart // is just hitting the floor, before being pushed up again by // the suspension. So we just do a longer initial simulation, // which should be long enough for all karts to be firmly on ground. - for(int i=0; i<200; i++) m_physics->update(1.f/60.f); + for(int i=0; i<60; i++) m_physics->update(1.f/60.f); // Stil wait will all karts are in rest (and handle the case that a kart // fell through the ground, which can happen if a kart falls for a long diff --git a/src/physics/btKart.cpp b/src/physics/btKart.cpp index 503a4c3ed..bb7172221 100644 --- a/src/physics/btKart.cpp +++ b/src/physics/btKart.cpp @@ -9,6 +9,10 @@ * It is provided "as is" without express or implied warranty. */ +/* Based on btRayCastVehicle, but modified for STK. + * projectVehicleToSurface function and shorter raycast functions added. + */ + #include "physics/btKart.hpp" #include "LinearMath/btMinMax.h" @@ -130,6 +134,247 @@ btScalar btKart::rayCast(btWheelInfo& wheel) return depth; } + +// ---------------------------------------------------------------------------- +//Shorter version of above raycast function. This is used when projecting +//vehicles towards the ground at the start of a race +btScalar btKart::rayCast(btWheelInfo& wheel, const btVector3& ray) +{ + updateWheelTransformsWS( wheel,false); + + btScalar depth = -1; + + const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; + wheel.m_raycastInfo.m_contactPointWS = source + ray; + const btVector3& target = source + ray; + + btVehicleRaycaster::btVehicleRaycasterResult rayResults; + + assert(m_vehicleRaycaster); + + void* object = m_vehicleRaycaster->castRay(source,target,rayResults); + + wheel.m_raycastInfo.m_groundObject = 0; + + if (object) + { + depth = ray.length() * rayResults.m_distFraction; + + wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; + wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; + wheel.m_raycastInfo.m_isInContact = true; + } + + return depth; +} + +// ---------------------------------------------------------------------------- +//Project vehicle onto surface in a particular direction. +//Used in reseting kart positions. +//Please align wheel direction with ray direction first. +bool btKart::projectVehicleToSurface(const btVector3& ray, bool translate_vehicle) +{ + for (int i=0;i btScalar(0)) + ray_dir = ray / ray.length(); + + btTransform trans = getRigidBody()->getCenterOfMassTransform(); + btTransform rot_trans; + rot_trans.setIdentity(); + rot_trans.setRotation(trans.getRotation()); + rot_trans = rot_trans.inverse(); + + + btTransform offset_trans; + offset_trans.setIdentity(); + btVector3 offset= min_wheel.m_raycastInfo.m_hardPointWS + min_wheel.m_wheelsRadius * ray_dir; + offset -= getRigidBody()->getCenterOfMassPosition(); + offset_trans.setOrigin(rot_trans*offset); + + + //the effect of the following rotations is to make the 3 wheels with initial + //minimum distance to surface (in the ray direction) in contact with the + //plane between the points of intersection (between the ray and surface). + + //Note - For possible complex surfaces with lots of bumps directly under vehicle, + // the raycast needs to be done from a slightly higher above the surface. + // For such surfaces, the end result should be that no wheel is below the + // surface, but there can be not all wheels touching surface. + + //We need to rotate vehicle, using above contact point as a pivot to put + //2nd closest wheel nearer to the surface of the track + btScalar d_hpws = (min_wheel.m_raycastInfo.m_hardPointWS - min_wheel2.m_raycastInfo.m_hardPointWS).length(); + btScalar d_depth = (min_wheel2.m_raycastInfo.m_contactPointWS - min_wheel2.m_raycastInfo.m_hardPointWS - ray_dir * min_wheel.m_wheelsRadius).length(); + d_depth -= min_depth; + + //calculate rotation angle from pivot point and plane perpendicular to ray + float rot_angle = atanf(d_depth / d_hpws); + rot_angle -= atanf((min_wheel2.m_wheelsRadius - min_wheel.m_wheelsRadius) / d_hpws); + + getRigidBody()->setAngularVelocity(btVector3(0,0,0)); + getRigidBody()->setLinearVelocity(btVector3(0,0,0)); + + + btVector3 rot_axis = (min_wheel2.m_raycastInfo.m_hardPointWS - min_wheel.m_raycastInfo.m_hardPointWS).cross(ray_dir); + + btTransform operator_trans; + operator_trans.setIdentity(); + + //perform pivot rotation + if (rot_axis.length() != btScalar(0)) + { + //rotate kart about pivot point, about line perpendicular to + //ray and vector between the 2 wheels + operator_trans *= offset_trans; + operator_trans.setRotation(btQuaternion(rot_trans*rot_axis.normalize(), rot_angle)); + offset_trans.setOrigin(-(rot_trans*offset)); + operator_trans *= offset_trans; + } + + //apply tranform + trans *= operator_trans; + getRigidBody()->setCenterOfMassTransform(trans); + + //next, rotate about axis which is a vector between 2 wheels above, so that + //the 3rd wheel is correctly positioned. + + rot_axis = min_wheel2.m_raycastInfo.m_contactPointWS - min_wheel.m_raycastInfo.m_contactPointWS; + btVector3 wheel_dist = min_wheel3.m_raycastInfo.m_hardPointWS - min_wheel.m_raycastInfo.m_hardPointWS; + if (rot_axis.length() != btScalar(0)) + { + btVector3 proj = wheel_dist.dot(rot_axis) * rot_axis.normalize(); + + //calculate position on axis when a perpendicular line would go through + //3rd wheel position when translated in ray position and rotated as above + btVector3 pos_on_axis = min_wheel.m_raycastInfo.m_contactPointWS + proj; + + btVector3 to_contact_pt = min_wheel3.m_raycastInfo.m_contactPointWS - pos_on_axis; + btScalar dz = to_contact_pt.dot(ray_dir); + btScalar dw = (to_contact_pt - dz * ray_dir).length(); + rot_angle = atanf (dz / dw); + + btVector3 rot_point = getRigidBody()->getCenterOfMassPosition() + min_depth * ray_dir - min_wheel.m_raycastInfo.m_contactPointWS; + rot_point = rot_point.dot(rot_axis) * rot_axis.normalize() - rot_point; + + + //calculate translation offset to axis from center of mass along perpendicular + offset_trans.setIdentity(); + + offset= rot_point; + offset_trans.setOrigin(rot_trans*offset); + + btVector3 a = min_wheel3.m_raycastInfo.m_hardPointWS - min_wheel.m_raycastInfo.m_hardPointWS; + btVector3 b = min_wheel2.m_raycastInfo.m_hardPointWS - min_wheel.m_raycastInfo.m_hardPointWS; + + if ( (a.cross(b)).dot(ray_dir) > 0 ) + { + rot_angle *= btScalar(-1); + } + + //rotate about new axis + operator_trans.setIdentity(); + operator_trans *= offset_trans; + operator_trans.setRotation(btQuaternion(rot_trans*rot_axis.normalize(), rot_angle)); + offset_trans.setOrigin(-(rot_trans*offset)); + operator_trans *= offset_trans; + + //apply tranform + trans *= operator_trans; + getRigidBody()->setCenterOfMassTransform(trans); + } + + if (!translate_vehicle) + return true; + + min_depth = btScalar(-1); //minimum distance of wheel to surface + + for (int i=0;itranslate((min_depth - min_wheel.getSuspensionRestLength()) * ray_dir); + return true; +} + // ---------------------------------------------------------------------------- void btKart::updateVehicle( btScalar step ) { diff --git a/src/physics/btKart.hpp b/src/physics/btKart.hpp index 4be70f71c..66e0cbfa3 100644 --- a/src/physics/btKart.hpp +++ b/src/physics/btKart.hpp @@ -31,6 +31,8 @@ public: btVehicleRaycaster* raycaster, float track_connect_accel ); virtual ~btKart() ; btScalar rayCast(btWheelInfo& wheel); + btScalar rayCast(btWheelInfo& wheel, const btVector3& ray); + bool projectVehicleToSurface(const btVector3& ray, bool translate_vehicle); void setSkidding(btScalar sf) { m_skidding_factor = sf; } virtual void updateVehicle(btScalar step); void resetSuspension(); diff --git a/src/physics/physics.cpp b/src/physics/physics.cpp index 12e12b072..8f4162b71 100644 --- a/src/physics/physics.cpp +++ b/src/physics/physics.cpp @@ -152,6 +152,17 @@ void Physics::update(float dt) } // for all p in m_all_collisions } // update +//----------------------------------------------------------------------------- +/** Project all karts downwards onto the surface below. + * Used in setting the starting positions of all the karts. + */ + +bool Physics::projectKartDownwards(const Kart *k) +{ + btVector3 hell(0, 0, -10000); + return k->getVehicle()->projectVehicleToSurface(hell, true /*allow translation*/); +} //projectKartsDownwards + //----------------------------------------------------------------------------- /** Handles the special case of two karts colliding with each other, which means * that bombs must be passed on. If both karts have a bomb, they'll explode diff --git a/src/physics/physics.hpp b/src/physics/physics.hpp index 5746bb538..14e149cb8 100644 --- a/src/physics/physics.hpp +++ b/src/physics/physics.hpp @@ -106,6 +106,7 @@ public: btDynamicsWorld* getPhysicsWorld () const {return m_dynamics_world;} void debugDraw (float m[16], btCollisionShape *s, const btVector3 color); + bool projectKartDownwards(const Kart *k); virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold,int numManifolds, btTypedConstraint** constraints,int numConstraints,