2011-02-18 14:31:32 +00:00
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
Copyright ( C ) 1999 - 2005 Id Software , Inc .
Copyright ( C ) 2002 - 2009 Q3Rally Team ( Per Thormann - perle @ q3rally . com )
This file is part of q3rally source code .
q3rally source code is free software ; you can redistribute it
and / or modify it under the terms of the GNU General Public License as
published by the Free Software Foundation ; either version 2 of the License ,
or ( at your option ) any later version .
q3rally source code is distributed in the hope that it will be
useful , but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with q3rally ; if not , write to the Free Software
Foundation , Inc . , 51 Franklin St , Fifth Floor , Boston , MA 02110 - 1301 USA
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
# include "../qcommon/q_shared.h"
# ifdef GAME
# include "g_local.h"
# else
# include "bg_public.h"
//#include "../cgame/cg_local.h"
# endif
# include "bg_local.h"
float CP_CURRENT_GRAVITY ;
// not actually used now, use cvars instead
float CP_SPRING_STRENGTH = 110 * ( CP_FRAME_MASS / 350.0f ) * CP_GRAVITY ; // 110
float CP_SHOCK_STRENGTH = 13 * CP_GRAVITY ; // 12
float CP_SWAYBAR_STRENGTH = 21 * CP_GRAVITY ; // 20 * grav
float CP_M_2_QU = CP_FT_2_QU / CP_FT_2_M ; // 35.66
float CP_WR_STRENGTH = 2400.0f * CP_WHEEL_MASS ;
float CP_WR_DAMP_STRENGTH = 140.0f * CP_WHEEL_MASS ;
static int numTraces ;
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_DebugDynamics
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
void PM_DebugDynamics ( carBody_t * body , carPoint_t * points ) {
Com_Printf ( " \n " ) ;
Com_Printf ( " PM_DebugDynamics: point ORIGIN - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . r [ 0 ] , points [ pm - > pDebug - 1 ] . r [ 1 ] , points [ pm - > pDebug - 1 ] . r [ 2 ] ) ;
Com_Printf ( " PM_DebugDynamics: point VELOCITY - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . v [ 0 ] , points [ pm - > pDebug - 1 ] . v [ 1 ] , points [ pm - > pDebug - 1 ] . v [ 2 ] ) ;
Com_Printf ( " PM_DebugDynamics: point ONGROUND - %i \n " , points [ pm - > pDebug - 1 ] . onGround ) ;
Com_Printf ( " PM_DebugDynamics: point NORMAL 1- %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . normals [ 0 ] [ 0 ] , points [ pm - > pDebug - 1 ] . normals [ 0 ] [ 1 ] , points [ pm - > pDebug - 1 ] . normals [ 0 ] [ 2 ] ) ;
Com_Printf ( " PM_DebugDynamics: point NORMAL 2- %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . normals [ 1 ] [ 0 ] , points [ pm - > pDebug - 1 ] . normals [ 1 ] [ 1 ] , points [ pm - > pDebug - 1 ] . normals [ 1 ] [ 2 ] ) ;
Com_Printf ( " PM_DebugDynamics: point NORMAL 3- %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . normals [ 2 ] [ 0 ] , points [ pm - > pDebug - 1 ] . normals [ 2 ] [ 1 ] , points [ pm - > pDebug - 1 ] . normals [ 2 ] [ 2 ] ) ;
// Com_Printf("PM_DebugDynamics: point MASS - %.3f\n", points[pm->pDebug-1].mass);
Com_Printf ( " PM_DebugDynamics: body ORIGIN - %.3f, %.3f, %.3f \n " , body - > r [ 0 ] , body - > r [ 1 ] , body - > r [ 2 ] ) ;
Com_Printf ( " PM_DebugDynamics: body VELOCITY - %.3f, %.3f, %.3f \n " , body - > v [ 0 ] , body - > v [ 1 ] , body - > v [ 2 ] ) ;
Com_Printf ( " PM_DebugDynamics: body ANG VELOCITY - %.3f, %.3f, %.3f \n " , body - > w [ 0 ] , body - > w [ 1 ] , body - > w [ 2 ] ) ;
// Com_Printf("PM_DebugDynamics: body COM - %.3f, %.3f, %.3f\n", body->CoM[0], body->CoM[1], body->CoM[2]);
// Com_Printf("PM_DebugDynamics: body MASS - %.3f\n", body->mass);
Com_Printf ( " Direction vectors ---------------------------------------- \n " ) ;
Com_Printf ( " PM_DebugDynamics: body FORWARD - %.3f, %.3f, %.3f \n " , body - > forward [ 0 ] , body - > forward [ 1 ] , body - > forward [ 2 ] ) ;
Com_Printf ( " PM_DebugDynamics: body RIGHT - %.3f, %.3f, %.3f \n " , body - > right [ 0 ] , body - > right [ 1 ] , body - > right [ 2 ] ) ;
Com_Printf ( " PM_DebugDynamics: body UP - %.3f, %.3f, %.3f \n " , body - > up [ 0 ] , body - > up [ 1 ] , body - > up [ 2 ] ) ;
Com_Printf ( " \n " ) ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_DebugForces
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
void PM_DebugForces ( carBody_t * body , carPoint_t * points ) {
Com_Printf ( " \n " ) ;
Com_Printf ( " PM_DebugForces: point GRAVITY - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . forces [ GRAVITY ] [ 0 ] , points [ pm - > pDebug - 1 ] . forces [ GRAVITY ] [ 1 ] , points [ pm - > pDebug - 1 ] . forces [ GRAVITY ] [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: point NORMAL - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . forces [ NORMAL ] [ 0 ] , points [ pm - > pDebug - 1 ] . forces [ NORMAL ] [ 1 ] , points [ pm - > pDebug - 1 ] . forces [ NORMAL ] [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: point SHOCK - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . forces [ SHOCK ] [ 0 ] , points [ pm - > pDebug - 1 ] . forces [ SHOCK ] [ 1 ] , points [ pm - > pDebug - 1 ] . forces [ SHOCK ] [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: point SPRING - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . forces [ SPRING ] [ 0 ] , points [ pm - > pDebug - 1 ] . forces [ SPRING ] [ 1 ] , points [ pm - > pDebug - 1 ] . forces [ SPRING ] [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: point ROAD - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . forces [ ROAD ] [ 0 ] , points [ pm - > pDebug - 1 ] . forces [ ROAD ] [ 1 ] , points [ pm - > pDebug - 1 ] . forces [ ROAD ] [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: point INTERNAL - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . forces [ INTERNAL ] [ 0 ] , points [ pm - > pDebug - 1 ] . forces [ INTERNAL ] [ 1 ] , points [ pm - > pDebug - 1 ] . forces [ INTERNAL ] [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: point AIR_FRICTION - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . forces [ AIR_FRICTION ] [ 0 ] , points [ pm - > pDebug - 1 ] . forces [ AIR_FRICTION ] [ 1 ] , points [ pm - > pDebug - 1 ] . forces [ AIR_FRICTION ] [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: point NETFORCE - %.3f, %.3f, %.3f \n " , points [ pm - > pDebug - 1 ] . netForce [ 0 ] , points [ pm - > pDebug - 1 ] . netForce [ 1 ] , points [ pm - > pDebug - 1 ] . netForce [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: body netForce - %.3f, %.3f, %.3f \n " , body - > netForce [ 0 ] , body - > netForce [ 1 ] , body - > netForce [ 2 ] ) ;
Com_Printf ( " PM_DebugForces: body netMoment - %.3f, %.3f, %.3f \n " , body - > netMoment [ 0 ] , body - > netMoment [ 1 ] , body - > netMoment [ 2 ] ) ;
Com_Printf ( " \n " ) ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_CopyTargetToSource
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static void PM_CopyTargetToSource ( carBody_t * tBody , carBody_t * sBody , carPoint_t * tPoints , carPoint_t * sPoints ) {
memcpy ( sBody , tBody , sizeof ( carBody_t ) ) ;
memcpy ( sPoints , tPoints , NUM_CAR_POINTS * sizeof ( carPoint_t ) ) ;
}
/*
= = = = = = = = = = = = = = = = = = =
PM_SetCoM
= = = = = = = = = = = = = = = = = = =
*/
void PM_SetCoM ( carBody_t * body , carPoint_t * points ) {
vec3_t temp , origin ;
//vec3_t delta;
float totalMass , length ;
int i ;
VectorClear ( temp ) ;
totalMass = CP_CAR_MASS ;
for ( i = FIRST_FRAME_POINT ; i < LAST_FRAME_POINT ; i + + ) {
VectorMA ( temp , points [ i ] . mass , points [ i ] . r , temp ) ;
// totalMass += points[i].mass;
}
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + ) {
// VectorSubtract( points[i+4].r, points[i].r, delta );
// length = DotProduct( body->up, delta );
length = body - > curSpringLengths [ i ] ;
if ( length < CP_SPRING_MINLEN )
{
length = CP_SPRING_MINLEN ;
VectorMA ( points [ i + 4 ] . r , length , body - > up , origin ) ;
VectorMA ( temp , points [ i ] . mass , origin , temp ) ;
}
else if ( length > CP_SPRING_MAXLEN )
{
length = CP_SPRING_MAXLEN ;
VectorMA ( points [ i + 4 ] . r , length , body - > up , origin ) ;
VectorMA ( temp , points [ i ] . mass , origin , temp ) ;
}
else
VectorMA ( temp , points [ i ] . mass , points [ i ] . r , temp ) ;
// totalMass += points[i].mass;
}
VectorScale ( temp , 1.0f / totalMass , body - > CoM ) ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
POINT FUNCTIONS
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_ClearCarForces
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static void PM_ClearCarForces ( carBody_t * body , carPoint_t * points ) {
int i , j ;
VectorClear ( body - > netForce ) ;
VectorClear ( body - > netMoment ) ;
for ( i = 0 ; i < NUM_CAR_POINTS ; i + + ) {
VectorClear ( points [ i ] . netForce ) ;
points [ i ] . netMoment = 0 ;
for ( j = 0 ; j < NUM_CAR_FORCES ; j + + ) {
VectorClear ( points [ i ] . forces [ j ] ) ;
}
}
}
/*
= = = = = = = = = = = = = = = = = = =
PM_CalculateNetForce
= = = = = = = = = = = = = = = = = = =
*/
void PM_CalculateNetForce ( carPoint_t * point , int pointIndex ) {
int i ;
float dot ;
// vec3_t vecForce, dV;
VectorClear ( point - > netForce ) ;
for ( i = 0 ; i < NUM_CAR_FORCES ; i + + ) {
if ( i = = NORMAL ) continue ;
/*
if ( VectorLength ( point - > forces [ i ] ) > = 1 < < 23 ) {
Com_Printf ( " PM_CalculateNetForce: Car point force %d is greater than %d \n " , i , 1 < < 23 ) ;
Com_Printf ( " PM_CalculateNetForce: Car point force is %f \n " , VectorLength ( point - > forces [ i ] ) ) ;
VectorNormalize ( point - > forces [ i ] ) ;
VectorScale ( point - > forces [ i ] , 1 < < 23 , point - > forces [ i ] ) ;
}
*/
if ( VectorNAN ( point - > forces [ i ] ) )
{
Com_Printf ( " Force %i on point %i was NAN \n " , i , pointIndex ) ;
continue ;
}
VectorAdd ( point - > netForce , point - > forces [ i ] , point - > netForce ) ;
}
VectorClear ( point - > forces [ NORMAL ] ) ;
// VectorClear( vecForce );
// calculate normal force
if ( point - > onGround )
{
for ( i = 0 ; i < 3 ; i + + )
{
if ( VectorLengthSquared ( point - > normals [ i ] ) < 10e-2 ) continue ;
// calculate new velocity of wheel after collision
dot = DotProduct ( point - > normals [ i ] , point - > v ) ;
if ( dot < - 10.0f )
{
// VectorScale(point->normals[i], -(1 + point->elasticity) * dot, dV);
// VectorAdd(point->v, dV, point->v);
VectorMA ( point - > v , - ( 1.0f + point - > elasticity ) * dot , point - > normals [ i ] , point - > v ) ;
}
else if ( dot < 1.0f )
{
// going slow enough so just stop it so it doesnt vibrate
// VectorScale(point->normals[i], -dot, dV);
// VectorAdd(point->v, dV, point->v);
VectorMA ( point - > v , - dot , point - > normals [ i ] , point - > v ) ;
}
// add normal force to balance the other forces
dot = OVERCLIP * DotProduct ( point - > normals [ i ] , point - > netForce ) ;
if ( dot < 0 )
{
// VectorScale(point->normals[i], -dot, vecForce);
// VectorAdd(point->forces[NORMAL], vecForce, point->forces[NORMAL]);
VectorMA ( point - > forces [ NORMAL ] , - dot , point - > normals [ i ] , point - > forces [ NORMAL ] ) ;
}
}
}
/*
if ( VectorLength ( point - > forces [ NORMAL ] ) > = 1 < < 23 ) {
Com_Printf ( " PM_CalculateNetForce: Car point normal force is greater than %d \n " , 1 < < 23 ) ;
VectorNormalize ( point - > forces [ NORMAL ] ) ;
VectorScale ( point - > forces [ NORMAL ] , 1 < < 23 , point - > forces [ NORMAL ] ) ;
}
*/
VectorAdd ( point - > netForce , point - > forces [ NORMAL ] , point - > netForce ) ;
}
/*
= = = = = = = = = = = = = = = = = = =
PM_AccelerateAndMove
= = = = = = = = = = = = = = = = = = =
*/
static void PM_AccelerateAndMove ( car_t * car , carPoint_t * source , carPoint_t * target , float time )
{
float alpha ;
vec3_t vec , avgAccel ;
// vec3_t avgVel;
// kill car if forces too high
if ( VectorNAN ( source - > netForce ) )
{
Com_Printf ( " Blowing up car because of car point force \n " ) ;
pm - > damage . damage = 32000 ;
pm - > damage . dflags = DAMAGE_NO_PROTECTION ;
pm - > damage . otherEnt = - 1 ;
pm - > damage . mod = MOD_HIGH_FORCES ;
return ;
}
// using const acceleration
// NOTE: this method is really not right but it works so oh well.
// target has last netForce still in it
VectorScale ( source - > netForce , 0.4f / source - > mass , avgAccel ) ;
VectorMA ( source - > v , time , avgAccel , target - > v ) ;
VectorAdd ( source - > v , target - > v , vec ) ;
VectorMA ( source - > r , time / 2 , vec , target - > r ) ;
// UPDATE: use a moment of inertia different than .5*mr^2?
alpha = ( source - > netMoment + target - > netMoment ) / ( 0.75f * source - > mass * WHEEL_RADIUS * WHEEL_RADIUS ) ;
target - > w = source - > w + ( time / 6.0f ) * alpha ;
if ( source - > w < 0 & & target - > w > 0 )
{
target - > w = 0 ;
target - > netMoment = 0 ;
}
if ( source - > w > 0 & & target - > w < 0 )
{
target - > w = 0 ;
target - > netMoment = 0 ;
}
// wheels are trying to turn in the wrong direction..
// FIXME: use force in opposite diretion instead of just
// stopping the wheels from turning.
// Play transmission grinding sound if the force is too big
// PHYSICS TEMP
if ( car - > gear > 0 & & target - > w > - 1.5f )
{
// Com_Printf("You're screwing up your transmission %f\n", target->w);
target - > w = 0 ;
}
else if ( car - > gear < 0 & & target - > w < 1.5f )
{
// Com_Printf("You're screwing up your transmission %f\n", target->w);
target - > w = 0 ;
}
target - > slipping = source - > slipping ;
// copy netForce and netMoment to target so we can use it during the AccelerateandMove next frame
VectorCopy ( source - > netForce , target - > netForce ) ;
target - > netMoment = source - > netMoment ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
INITIALIZATION FUNCTIONS
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
/*
= = = = = = = = = = = = = = = = = = =
PM_UpdateFrameVelocities
= = = = = = = = = = = = = = = = = = =
*/
static void PM_UpdateFrameVelocities ( carBody_t * body , carPoint_t * points )
{
vec3_t arm ;
vec3_t cross ;
int i ;
for ( i = FIRST_FRAME_POINT ; i < NUM_CAR_POINTS ; i + + )
{
VectorSubtract ( points [ i ] . r , body - > CoM , arm ) ;
CrossProduct ( body - > w , arm , cross ) ;
VectorAdd ( body - > v , cross , points [ i ] . v ) ;
}
}
/*
= = = = = = = = = = = = = = = = = = =
PM_InitializeFrame
= = = = = = = = = = = = = = = = = = =
*/
static void PM_InitializeFrame ( carBody_t * body , carPoint_t * point , float f , float r , float u )
{
vec3_t arm ;
vec3_t cross ;
VectorScale ( body - > forward , f * WHEEL_FORWARD , arm ) ;
VectorMA ( arm , r * WHEEL_RIGHT , body - > right , arm ) ;
VectorMA ( arm , - u * WHEEL_UP , body - > up , arm ) ;
VectorAdd ( body - > r , arm , point - > r ) ;
CrossProduct ( body - > w , arm , cross ) ;
VectorAdd ( body - > v , cross , point - > v ) ;
VectorClear ( point - > normals [ 0 ] ) ;
VectorClear ( point - > normals [ 1 ] ) ;
VectorClear ( point - > normals [ 2 ] ) ;
point - > onGround = qfalse ;
}
/*
= = = = = = = = = = = = = = = = = = =
PM_InitializeWheel
= = = = = = = = = = = = = = = = = = =
*/
static void PM_InitializeWheel ( carBody_t * body , carPoint_t * points , int i , int f , int r )
{
vec3_t arm ;
vec3_t cross ;
VectorScale ( body - > forward , f * WHEEL_FORWARD , arm ) ;
VectorMA ( arm , r * WHEEL_RIGHT , body - > right , arm ) ;
VectorMA ( arm , WHEEL_UP , body - > up , arm ) ;
VectorAdd ( body - > r , arm , points [ i ] . r ) ;
CrossProduct ( body - > w , arm , cross ) ;
VectorAdd ( body - > v , cross , points [ i ] . v ) ;
VectorClear ( points [ i ] . normals [ 0 ] ) ;
VectorClear ( points [ i ] . normals [ 1 ] ) ;
VectorClear ( points [ i ] . normals [ 2 ] ) ;
// UPDATE: added for client prediction, should just leave normals instead?
points [ i ] . onGround = qfalse ;
// recalculate spring length
VectorSubtract ( points [ i + 4 ] . r , points [ i ] . r , arm ) ;
body - > curSpringLengths [ i ] = DotProduct ( arm , body - > up ) ;
}
/*
= = = = = = = = = = = = = = = = = = =
PM_InitializeVehicle
= = = = = = = = = = = = = = = = = = =
*/
void PM_InitializeVehicle ( car_t * car , vec3_t origin , vec3_t angles , vec3_t velocity ) {
float m [ 3 ] [ 3 ] ;
int i ;
float halfWidth , halfLength , halfHeight ;
float forwardScale , rightScale , upScale ;
// UPDATE: use memset?
VectorCopy ( origin , car - > sBody . r ) ;
AnglesToOrientation ( angles , car - > sBody . t ) ;
// AnglesToQuaternion(angles, car->sBody.q);
OrientationToVectors ( car - > sBody . t , car - > sBody . forward , car - > sBody . right , car - > sBody . up ) ;
VectorCopy ( velocity , car - > sBody . v ) ;
VectorClear ( car - > sBody . w ) ;
VectorClear ( car - > sBody . L ) ;
// set locations and velocities of frame points
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ FL_FRAME ] , 1.0f , - 1.0f , 0.0f ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ FR_FRAME ] , 1.0f , 1.0f , 0.0f ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ RL_FRAME ] , - 1.0f , - 1.0f , 0.0f ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ RR_FRAME ] , - 1.0f , 1.0f , 0.0f ) ;
// body collision points
forwardScale = ( ( CAR_LENGTH / 2.0f ) - BODY_RADIUS ) / WHEEL_FORWARD ;
rightScale = ( ( CAR_WIDTH / 2.0f ) - BODY_RADIUS ) / WHEEL_RIGHT ;
upScale = ( 3.0f ) / - WHEEL_UP ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ FL_BODY ] , forwardScale , - rightScale , upScale ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ FR_BODY ] , forwardScale , rightScale , upScale ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ ML_BODY ] , 0.0f , - rightScale , upScale ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ MR_BODY ] , 0.0f , rightScale , upScale ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ RL_BODY ] , - forwardScale , - rightScale , upScale ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ RR_BODY ] , - forwardScale , rightScale , upScale ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ ML_ROOF ] , 0.0f , - rightScale , 0.6f ) ;
PM_InitializeFrame ( & car - > sBody , & car - > sPoints [ MR_ROOF ] , 0.0f , rightScale , 0.6f ) ;
// set locations and velocities of wheel points
PM_InitializeWheel ( & car - > sBody , car - > sPoints , FL_WHEEL , 1.0f , - 1.0f ) ;
PM_InitializeWheel ( & car - > sBody , car - > sPoints , FR_WHEEL , 1.0f , 1.0f ) ;
PM_InitializeWheel ( & car - > sBody , car - > sPoints , RL_WHEEL , - 1.0f , - 1.0f ) ;
PM_InitializeWheel ( & car - > sBody , car - > sPoints , RR_WHEEL , - 1.0f , 1.0f ) ;
car - > sBody . mass = 4 * CP_FRAME_MASS ;
car - > tBody . mass = 4 * CP_FRAME_MASS ;
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + ) {
car - > sPoints [ i ] . mass = CP_WHEEL_MASS ;
car - > sPoints [ i ] . elasticity = CP_WHEEL_ELASTICITY ;
car - > sPoints [ i ] . scof = CP_SCOF ;
car - > sPoints [ i ] . kcof = CP_KCOF ;
car - > sPoints [ i ] . slipping = qfalse ;
car - > sPoints [ i ] . onGround = qfalse ;
car - > sPoints [ i ] . onGroundTime = 0 ;
car - > sPoints [ i ] . radius = WHEEL_RADIUS ;
car - > tPoints [ i ] . mass = CP_WHEEL_MASS ;
car - > tPoints [ i ] . elasticity = CP_WHEEL_ELASTICITY ;
car - > tPoints [ i ] . scof = CP_SCOF ;
car - > tPoints [ i ] . kcof = CP_KCOF ;
car - > tPoints [ i ] . slipping = qfalse ;
car - > tPoints [ i ] . onGround = qfalse ;
car - > tPoints [ i ] . onGroundTime = 0 ;
car - > tPoints [ i ] . radius = WHEEL_RADIUS ;
}
for ( ; i < NUM_CAR_POINTS ; i + + ) {
if ( i < LAST_FRAME_POINT ) {
if ( i < 6 )
{
car - > sPoints [ i ] . mass = 0.5f * car - > sBody . mass * pm - > car_frontweight_dist ;
car - > tPoints [ i ] . mass = 0.5f * car - > sBody . mass * pm - > car_frontweight_dist ;
}
else
{
car - > sPoints [ i ] . mass = 0.5f * car - > sBody . mass * ( 1.0f - pm - > car_frontweight_dist ) ;
car - > tPoints [ i ] . mass = 0.5f * car - > sBody . mass * ( 1.0f - pm - > car_frontweight_dist ) ;
}
}
else {
car - > sPoints [ i ] . mass = 0.0f ;
car - > tPoints [ i ] . mass = 0.0f ;
}
car - > sPoints [ i ] . scof = CP_SCOF ;
car - > sPoints [ i ] . kcof = CP_KCOF ;
car - > sPoints [ i ] . elasticity = pm - > car_body_elasticity ;
car - > sPoints [ i ] . slipping = qfalse ;
car - > sPoints [ i ] . onGround = qfalse ;
car - > sPoints [ i ] . onGroundTime = 0 ;
car - > sPoints [ i ] . radius = BODY_RADIUS ;
car - > tPoints [ i ] . scof = CP_SCOF ;
car - > tPoints [ i ] . kcof = CP_KCOF ;
car - > tPoints [ i ] . elasticity = pm - > car_body_elasticity ;
car - > tPoints [ i ] . slipping = qfalse ;
car - > tPoints [ i ] . onGround = qfalse ;
car - > tPoints [ i ] . onGroundTime = 0 ;
car - > tPoints [ i ] . radius = BODY_RADIUS ;
}
car - > sBody . elasticity = car - > sPoints [ 4 ] . elasticity ;
car - > tBody . elasticity = car - > sPoints [ 4 ] . elasticity ;
PM_SetCoM ( & car - > sBody , car - > sPoints ) ;
halfWidth = CAR_WIDTH / 2.0f ;
halfLength = CAR_LENGTH / 2.0f ;
halfHeight = CAR_HEIGHT / 2.0f ;
car - > inverseBodyInertiaTensor [ 0 ] [ 0 ] = pm - > car_IT_xScale * 3.0f / ( car - > sBody . mass * ( halfLength * halfLength + halfHeight * halfHeight ) ) ;
car - > inverseBodyInertiaTensor [ 1 ] [ 1 ] = pm - > car_IT_yScale * 3.0f / ( car - > sBody . mass * ( halfWidth * halfWidth + halfHeight * halfHeight ) ) ;
car - > inverseBodyInertiaTensor [ 2 ] [ 2 ] = pm - > car_IT_zScale * 3.0f / ( car - > sBody . mass * ( halfWidth * halfWidth + halfLength * halfLength ) ) ;
car - > springStrength = CP_SPRING_STRENGTH ;
car - > springMaxLength = CP_SPRING_MAXLEN ;
car - > springMinLength = CP_SPRING_MINLEN ;
// car->shockStrength = CP_SHOCK_STRENGTH;
car - > gear = 1 ;
car - > rpm = CP_RPM_MIN ;
// car->aCOF = CP_AIR_COF;
// car->dfCOF = CP_FRAC_TO_DF;
// car->ewCOF = CP_ENGINE_TIRE_COF;
MatrixMultiply ( car - > sBody . t , car - > inverseBodyInertiaTensor , car - > sBody . inverseWorldInertiaTensor ) ;
MatrixTranspose ( car - > sBody . t , m ) ;
MatrixMultiply ( car - > sBody . inverseWorldInertiaTensor , m , car - > sBody . inverseWorldInertiaTensor ) ;
PM_ClearCarForces ( & car - > sBody , car - > sPoints ) ;
PM_ClearCarForces ( & car - > tBody , car - > tPoints ) ;
PM_CopyTargetToSource ( & car - > sBody , & car - > tBody , car - > sPoints , car - > tPoints ) ;
}
/*
= = = = = = = = = = = = = = = = = = =
PM_SetFluidDensity
= = = = = = = = = = = = = = = = = = =
*/
static void PM_SetFluidDensity ( carPoint_t * points , int i ) {
vec3_t dest ;
int cont , level , type ;
// check waterlevel
level = 0 ;
type = 0 ;
VectorCopy ( points [ i ] . r , dest ) ;
dest [ 2 ] = points [ i ] . r [ 2 ] - ( points [ i ] . radius - 0.5f ) ;
cont = pm - > pointcontents ( dest , pm - > ps - > clientNum ) ;
if ( cont & MASK_WATER ) { // bottom of point is in the water
level + + ;
type = cont ;
}
dest [ 2 ] = points [ i ] . r [ 2 ] ;
cont = pm - > pointcontents ( dest , pm - > ps - > clientNum ) ;
if ( cont & MASK_WATER ) { // middle of point is in the water
level + + ;
type = cont ;
}
dest [ 2 ] = points [ i ] . r [ 2 ] + ( points [ i ] . radius - 0.5f ) ;
cont = pm - > pointcontents ( dest , pm - > ps - > clientNum ) ;
if ( cont & MASK_WATER ) { // top of point is in the water
level + + ;
type = cont ;
}
if ( level )
{
if ( type & CONTENTS_WATER ) {
points [ i ] . fluidDensity = ( level * CP_WATER_DENSITY + ( 3.0f - level ) * CP_AIR_DENSITY ) / 3.0f ;
}
else if ( type & CONTENTS_LAVA ) {
points [ i ] . fluidDensity = ( level * CP_LAVA_DENSITY + ( 3.0f - level ) * CP_AIR_DENSITY ) / 3.0f ;
}
else if ( type & CONTENTS_SLIME ) {
points [ i ] . fluidDensity = ( level * CP_SLIME_DENSITY + ( 3.0f - level ) * CP_AIR_DENSITY ) / 3.0f ;
}
else {
points [ i ] . fluidDensity = CP_AIR_DENSITY ;
}
// Com_Printf("PM_SetFluidDensity: level %i, density %.3f\n", level, points[i].fluidDensity);
}
else
{
points [ i ] . fluidDensity = CP_AIR_DENSITY ;
}
}
/*
= = = = = = = = = = = = = = = = = = =
PM_CheckSurfaceFlags
= = = = = = = = = = = = = = = = = = =
*/
static void PM_CheckSurfaceFlags ( trace_t * trace , carPoint_t * point ) {
// TODO: include rolling friction... harder to roll tires in sand or mud
if ( pm - > frictionFunc ( point , & point - > scof , & point - > kcof ) )
{
point - > kcof * = pm - > car_friction_scale ;
point - > scof * = pm - > car_friction_scale ;
return ;
}
else if ( trace - > surfaceFlags & SURF_SLICK ) {
point - > kcof = CP_ICE_KCOF ;
point - > scof = CP_ICE_SCOF ;
}
else if ( trace - > surfaceFlags & SURF_ICE ) {
point - > kcof = CP_ICE_KCOF ;
point - > scof = CP_ICE_SCOF ;
}
else if ( trace - > surfaceFlags & SURF_GRASS ) {
point - > kcof = CP_GRASS_KCOF ;
point - > scof = CP_GRASS_SCOF ;
}
else if ( trace - > surfaceFlags & SURF_DUST ) {
point - > kcof = CP_DIRT_KCOF ;
point - > scof = CP_DIRT_SCOF ;
}
else if ( trace - > surfaceFlags & SURF_SNOW ) {
point - > kcof = CP_SNOW_KCOF ;
point - > scof = CP_SNOW_SCOF ;
}
else if ( trace - > surfaceFlags & SURF_GRAVEL ) {
point - > kcof = CP_GRAVEL_KCOF ;
point - > scof = CP_GRAVEL_SCOF ;
}
else if ( trace - > surfaceFlags & SURF_DIRT ) {
point - > kcof = CP_DIRT_KCOF ;
point - > scof = CP_DIRT_SCOF ;
}
else {
point - > kcof = CP_KCOF ;
point - > scof = CP_SCOF ;
}
if ( trace - > surfaceFlags & SURF_WET ) {
point - > kcof * = CP_WET_SCALE ;
point - > scof * = CP_WET_SCALE ;
}
point - > kcof * = pm - > car_friction_scale ;
point - > scof * = pm - > car_friction_scale ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_ApplyForce
This function is largely based on the physics articles and
source from Chris Hecker : http : //www.d6.com/users/checker/dynamics.htm
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
void PM_ApplyForce ( carBody_t * body , vec3_t force , vec3_t at ) {
vec3_t arm , moment ;
VectorSubtract ( at , body - > CoM , arm ) ;
VectorAdd ( body - > netForce , force , body - > netForce ) ;
CrossProduct ( arm , force , moment ) ;
VectorAdd ( body - > netMoment , moment , body - > netMoment ) ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_ApplyCollision
Rigid body < - > world collision function .
This function is largely based on the physics articles and
source from Chris Hecker : http : //www.d6.com/users/checker/dynamics.htm
Given : rigid body , impact origin , impact normal , and elasticity
Find : the new linear and angular velocities as a result of the impact .
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static float PM_ApplyCollision ( carBody_t * body , carPoint_t * points , vec3_t at , vec3_t normal , float elasticity ) {
vec3_t arm ;
vec3_t vP1 ;
vec3_t impulse , impulseMoment ;
vec3_t delta , cross , cross2 ;
float impulseNum , oppositeImpulseNum , impulseDen , dot ;
int i ;
VectorSubtract ( at , body - > CoM , arm ) ;
// VectorSubtract(at, body->r, arm);
if ( pm - > pDebug ) {
Com_Printf ( " PM_ApplyCollision: arm %0.3f, %0.3f, %0.3f \n " , arm [ 0 ] , arm [ 1 ] , arm [ 2 ] ) ;
Com_Printf ( " PM_ApplyCollision: normal %0.3f, %0.3f, %0.3f \n " , normal [ 0 ] , normal [ 1 ] , normal [ 2 ] ) ;
}
CrossProduct ( body - > w , arm , cross ) ;
VectorAdd ( body - > v , cross , vP1 ) ;
// added from collision
VectorClear ( impulse ) ;
dot = DotProduct ( normal , vP1 ) ;
if ( dot < 0 ) {
impulseNum = - ( 1.0f + elasticity ) * dot ;
oppositeImpulseNum = - ( 1.0f - elasticity ) * dot ;
CrossProduct ( arm , normal , cross ) ;
VectorRotate ( cross , body - > inverseWorldInertiaTensor , cross2 ) ;
CrossProduct ( cross2 , arm , cross ) ;
// Com_Printf( "oneOverMass %f, cross.normal %f\n", 1.0f / body->mass, DotProduct(cross, normal) );
impulseDen = 1.0f / body - > mass + DotProduct ( cross , normal ) ;
VectorScale ( normal , impulseNum / impulseDen , impulse ) ;
}
else {
// not hitting surface
// Com_Printf( "PM_ApplyCollision: not hitting surface, %f\n", dot );
return 0.0f ;
}
// apply impulse to primary quantities
CrossProduct ( arm , impulse , impulseMoment ) ;
VectorScale ( impulse , 1.0 / body - > mass , impulse ) ;
VectorAdd ( body - > v , impulse , body - > v ) ;
VectorAdd ( body - > L , impulseMoment , body - > L ) ;
// compute affected auxiliary quantities
VectorRotate ( body - > L , body - > inverseWorldInertiaTensor , body - > w ) ;
// temp to help wheel movement when hitting walls
// FIXME: is this still needed?
VectorMA ( impulse , - DotProduct ( impulse , body - > up ) , body - > up , delta ) ;
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + ) {
VectorAdd ( points [ i ] . v , delta , points [ i ] . v ) ;
}
PM_UpdateFrameVelocities ( body , points ) ;
if ( fabs ( oppositeImpulseNum / impulseDen ) > 5000.0f )
return fabs ( oppositeImpulseNum / impulseDen ) ;
else
return 0 ;
}
#if 0
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_ApplyBodyBodyCollision
This function is a hacked up version of PM_ApplyCollision to try to simulate
the collision between two cars .
Given : 2 rigid bodies , impact origin , impact normal , and elasticity
Find : the new linear and angular velocities of the two objects as a result of the impact .
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static float PM_ApplyBodyBodyCollision ( carBody_t * body1 , carPoint_t * points1 , carBody_t * body2 , carPoint_t * points2 , vec3_t at , vec3_t normal , float elasticity ) {
//vec3_t arm1, arm2;
vec3_t vP1 , vP2 ;
//vec3_t impulse, impulseMoment;
//vec3_t cross, cross2;
vec3_t diff1 , diff2 , delta ;
//float impulseNum, oppositeImpulseNum, impulseDen;
//float totalMass;
int i ;
/*
totalMass = body1 - > mass + body2 - > mass ;
VectorSubtract ( at , body1 - > CoM , arm1 ) ;
VectorSubtract ( at , body2 - > CoM , arm2 ) ;
if ( pm - > pDebug ) {
Com_Printf ( " PM_ApplyBodyBodyCollision: arm1 %0.3f, %0.3f, %0.3f \n " , arm1 [ 0 ] , arm1 [ 1 ] , arm1 [ 2 ] ) ;
Com_Printf ( " PM_ApplyBodyBodyCollision: arm2 %0.3f, %0.3f, %0.3f \n " , arm2 [ 0 ] , arm2 [ 1 ] , arm2 [ 2 ] ) ;
Com_Printf ( " PM_ApplyBodyBodyCollision: normal %0.3f, %0.3f, %0.3f \n " , normal [ 0 ] , normal [ 1 ] , normal [ 2 ] ) ;
}
CrossProduct ( body1 - > w , arm1 , cross ) ;
VectorAdd ( body1 - > v , cross , vP1 ) ;
CrossProduct ( body2 - > w , arm2 , cross ) ;
VectorAdd ( body2 - > v , cross , vP2 ) ;
*/
// hacked up physics
VectorCopy ( body1 - > v , vP1 ) ;
VectorCopy ( body1 - > L , vP2 ) ;
PM_ApplyCollision ( body1 , points1 , at , normal , elasticity / 2.0f ) ;
// VectorMA( body1->v, body2->mass / totalMass, vP2, body1->v );
// Com_Printf( "PM_ApplyBodyBodyCollision: v before %0.3f, %0.3f, %0.3f\n", body2->v[0], body2->v[1], body2->v[2] );
VectorSubtract ( vP1 , body1 - > v , diff1 ) ;
VectorSubtract ( vP2 , body1 - > L , diff2 ) ;
VectorAdd ( body2 - > v , diff1 , body2 - > v ) ;
VectorAdd ( body2 - > L , diff2 , body2 - > L ) ;
// compute affected auxiliary quantities
VectorRotate ( body2 - > L , body2 - > inverseWorldInertiaTensor , body2 - > w ) ;
// temp to help wheel movement when hitting walls
// FIXME: is this still needed?
VectorMA ( diff1 , - DotProduct ( diff1 , body2 - > up ) , body2 - > up , delta ) ;
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + ) {
VectorAdd ( points2 [ i ] . v , delta , points2 [ i ] . v ) ;
}
PM_UpdateFrameVelocities ( body2 , points2 ) ;
// VectorInverse( normal );
// PM_ApplyCollision( body2, points2, at, normal, elasticity );
// VectorMA( body2->v, body1->mass / totalMass, vP1, body2->v );
// Com_Printf( "PM_ApplyBodyBodyCollision: v after %0.3f, %0.3f, %0.3f\n", body2->v[0], body2->v[1], body2->v[2] );
return 0 ;
/*
VectorSubtract ( vP1 , vP2 , vP1 ) ;
// added from collision
VectorClear ( impulse ) ;
// if (DotProduct(normal, vP1) < 0){
// massFraction = (2.0f * body1->mass) / (body1->mass + body2->mass);
// impulseNum = massFraction * DotProduct(normal, vP1);
// massFraction = ((1.0f + elasticity) * body1->mass) / (body1->mass + body2->mass);
impulseNum = - ( 1 + elasticity ) * DotProduct ( normal , vP1 ) ;
oppositeImpulseNum = - ( 1.0f - elasticity ) * DotProduct ( normal , vP1 ) ;
// impulseDen = 1.0f / body1->mass;
impulseDen = 1.0f / body1 - > mass + 1.0f / body2 - > mass ;
CrossProduct ( arm1 , normal , cross ) ;
VectorRotate ( cross , body1 - > inverseWorldInertiaTensor , cross2 ) ;
CrossProduct ( cross2 , arm1 , cross ) ;
//impulseDen = 1.0f / body1->mass + DotProduct(cross, normal);
impulseDen + = DotProduct ( cross , normal ) ;
CrossProduct ( arm2 , normal , cross ) ;
VectorRotate ( cross , body2 - > inverseWorldInertiaTensor , cross2 ) ;
CrossProduct ( cross2 , arm2 , cross ) ;
impulseDen + = DotProduct ( cross , normal ) ;
VectorScale ( normal , impulseNum / impulseDen , impulse ) ;
// }
// else {
// // not hitting surface
// return;
// }
// apply impulse to primary quantities
VectorMA ( body1 - > v , 1.0 / body1 - > mass , impulse , body1 - > v ) ;
CrossProduct ( arm1 , impulse , impulseMoment ) ;
VectorAdd ( body1 - > L , impulseMoment , body1 - > L ) ;
// compute affected auxiliary quantities
VectorRotate ( body1 - > L , body1 - > inverseWorldInertiaTensor , body1 - > w ) ;
// apply impulse to primary quantities of second object
VectorInverse ( impulse ) ;
VectorMA ( body2 - > v , 1.0 / body2 - > mass , impulse , body2 - > v ) ;
CrossProduct ( arm2 , impulse , impulseMoment ) ;
VectorAdd ( body2 - > L , impulseMoment , body2 - > L ) ;
// compute affected auxiliary quantities of second object
VectorRotate ( body2 - > L , body2 - > inverseWorldInertiaTensor , body2 - > w ) ;
// temp to help wheel movement when hitting walls
// VectorMA(impulse, -DotProduct(impulse, body->up), body->up, delta);
// for (i = 0; i < FIRST_FRAME_POINT; i++){
// VectorMA(points[i].v, 1.0 / body->mass, delta, points[i].v);
// }
PM_UpdateFrameVelocities ( body1 , points1 ) ;
if ( fabs ( oppositeImpulseNum / impulseDen ) > 5000.0f )
return fabs ( oppositeImpulseNum / impulseDen ) ;
else
return 0 ;
*/
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_ApplyPointBodyCollision
This function is a hacked up version of PM_ApplyCollision to try to simulate
the collision between a tire and the body of a car ( like when the suspension bottoms out ) .
Given : rigid body , impact origin , impact normal , and elasticity
Find : the new linear and angular velocities as a result of the impact .
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static void PM_ApplyPointBodyCollision ( carBody_t * body , carPoint_t * points , carPoint_t * pt , vec3_t at , vec3_t normal , float elasticity ) {
vec3_t arm ;
vec3_t vP1 ;
vec3_t impulse , impulseMoment ;
vec3_t cross , cross2 ;
float impulseNum , impulseDen ;
float massFraction ;
VectorSubtract ( at , body - > CoM , arm ) ;
// VectorSubtract(at, body->r, arm);
if ( pm - > pDebug ) {
Com_Printf ( " PM_ApplyPointBodyCollision: arm %0.3f, %0.3f, %0.3f \n " , arm [ 0 ] , arm [ 1 ] , arm [ 2 ] ) ;
Com_Printf ( " PM_ApplyPointBodyCollision: normal %0.3f, %0.3f, %0.3f \n " , normal [ 0 ] , normal [ 1 ] , normal [ 2 ] ) ;
}
CrossProduct ( body - > w , arm , cross ) ;
VectorAdd ( body - > v , cross , vP1 ) ;
// added from collision
VectorClear ( impulse ) ;
// if (DotProduct(normal, vP1) < 0){
massFraction = ( 2.0f * pt - > mass ) / ( pt - > mass + body - > mass ) ;
impulseNum = massFraction * ( DotProduct ( normal , pt - > v ) - DotProduct ( normal , vP1 ) ) ;
CrossProduct ( arm , normal , cross ) ;
VectorRotate ( cross , body - > inverseWorldInertiaTensor , cross2 ) ;
CrossProduct ( cross2 , arm , cross ) ;
impulseDen = 1.0 / body - > mass + DotProduct ( cross , normal ) ;
VectorScale ( normal , impulseNum / impulseDen , impulse ) ;
// }
// apply impulse to primary quantities of rigid body
VectorMA ( body - > v , 1.0 / body - > mass , impulse , body - > v ) ;
CrossProduct ( arm , impulse , impulseMoment ) ;
VectorAdd ( body - > L , impulseMoment , body - > L ) ;
// compute affected auxiliary quantities
VectorRotate ( body - > L , body - > inverseWorldInertiaTensor , body - > w ) ;
VectorMA ( pt - > v , - 1.0 / pt - > mass , impulse , pt - > v ) ;
/*
// temp to help wheel movement when hitting walls
VectorMA ( impulse , - DotProduct ( impulse , body - > up ) , body - > up , delta ) ;
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + ) {
VectorMA ( points [ i ] . v , 1.0 / body - > mass , delta , points [ i ] . v ) ;
}
*/
PM_UpdateFrameVelocities ( body , points ) ;
}
# endif
# ifdef CGAME
void CG_Sparks ( const vec3_t origin , const vec3_t normal , const vec3_t direction , const float speed ) ;
# endif
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_CarBodyFrictionForces
Friction forces between the car body and the world
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static void PM_CarBodyFrictionForces ( car_t * car , carBody_t * body , carPoint_t * points , int i ) {
vec3_t vel , force ;
float normalForce ;
float speed ;
normalForce = DotProduct ( body - > netForce , points [ i ] . normals [ 0 ] ) ;
if ( normalForce > = 0.0f ) return ;
VectorMA ( body - > netForce , - normalForce , points [ i ] . normals [ 0 ] , force ) ;
VectorMA ( points [ i ] . v , - DotProduct ( points [ i ] . v , points [ i ] . normals [ 0 ] ) , points [ i ] . normals [ 0 ] , vel ) ;
speed = VectorNormalize ( vel ) ;
if ( VectorLength ( force ) > fabs ( 0.7f * normalForce ) | | speed > 2.0f ) {
// VectorNormalize(vel);
VectorScale ( vel , 0.4f * normalForce , force ) ;
}
else {
VectorInverse ( force ) ;
}
PM_ApplyForce ( body , force , points [ i ] . r ) ;
# ifdef CGAME
if ( speed > 20.0f )
{
vec3_t origin ;
float r ;
r = random ( ) * 5000 / speed ;
if ( r < 80 )
{
VectorMA ( points [ i ] . r , - BODY_RADIUS / 1.5f , points [ i ] . normals [ 0 ] , origin ) ;
// VectorInverse( vel );
VectorMA ( vel , 0.1f , points [ i ] . normals [ 0 ] , vel ) ;
VectorNormalize ( vel ) ;
CG_Sparks ( origin , points [ i ] . normals [ 0 ] , vel , speed * 0.5f ) ;
}
}
# endif
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_Generate_SwayBar_Forces
Simulates ARBs in the car
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static void PM_Generate_SwayBar_Forces ( car_t * car , carBody_t * body , carPoint_t * points , int leftWheel , int rightWheel ) {
// vec3_t lSpring, rSpring;
// vec3_t lengthDiff;
float dot , force ;
// carPoint_t *lWheel;
// carPoint_t *rWheel;
// lWheel = &points[leftWheel];
// rWheel = &points[rightWheel];
if ( fabs ( DotProduct ( body - > v , body - > forward ) ) < 50.0f )
return ;
// VectorSubtract(points[leftWheel+4].v, lWheel->v, lSpring);
// VectorSubtract(points[rightWheel+4].v, rWheel->v, rSpring);
// VectorSubtract(lSpring, rSpring, lengthDiff);
dot = body - > curSpringLengths [ leftWheel ] - body - > curSpringLengths [ rightWheel ] ;
// if dotproduct(lengthDiff, up) < 0, rSpring > lSpring
// else rSpring < lSpring
// dot = DotProduct(body->up, lengthDiff);
force = dot * pm - > car_swaybar * CP_GRAVITY ;
if ( force < 0 & & ! points [ leftWheel ] . onGround )
return ;
else if ( force > 0 & & ! points [ rightWheel ] . onGround )
return ;
if ( force < 0 & & pm - > ps - > viewangles [ ROLL ] > 0 ) {
// Com_Printf("Sway bars shouldnt be forcing\n");
return ;
}
else if ( force > 0 & & pm - > ps - > viewangles [ ROLL ] < 0 ) {
// Com_Printf("Sway bars shouldnt be forcing\n");
return ;
}
// Com_Printf("PM_Generate_SwayBar_Forces: force %f, roll %f\n", force, pm->ps->viewangles[ROLL]);
// VectorScale(body->up, force, lWheel->forces[SWAY_BAR]);
VectorScale ( body - > up , - force , points [ leftWheel + 4 ] . forces [ SWAY_BAR ] ) ;
// VectorScale(body->up, -force, rWheel->forces[SWAY_BAR]);
VectorScale ( body - > up , force , points [ rightWheel + 4 ] . forces [ SWAY_BAR ] ) ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_Generate_FrameWheel_Forces
Handles spring and damper physics for the suspension
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static int PM_Generate_FrameWheel_Forces ( car_t * car , carPoint_t * points , int frameIndex , int wheelIndex ) {
vec3_t delta , diff ;
float compression , springVel , length ;
int hitType ;
carPoint_t * frame , * wheel ;
hitType = HTYPE_NO_HIT ;
frame = & points [ frameIndex ] ;
wheel = & points [ wheelIndex ] ;
//
// Calculate forces on the frame
//
// spring force based on length of spring (origin difference of the two points)
length = car - > sBody . curSpringLengths [ wheelIndex ] ;
// shock force based on velocity difference of the two points
VectorSubtract ( frame - > v , wheel - > v , delta ) ;
springVel = DotProduct ( car - > sBody . up , delta ) ;
if ( springVel > 0 ) {
VectorScale ( car - > sBody . up , - pm - > car_shock_up * CP_GRAVITY * springVel , frame - > forces [ SHOCK ] ) ;
}
else {
VectorScale ( car - > sBody . up , - pm - > car_shock_down * CP_GRAVITY * springVel , frame - > forces [ SHOCK ] ) ;
}
// compression = springVel > 0 ? 0 : springVel;
// VectorScale(car->sBody.up, -car->shockStrength * compression, frame->forces[SHOCK]);
if ( length < car - > springMinLength ) {
if ( springVel < 0 ) {
VectorScale ( car - > sBody . up , - springVel , diff ) ;
VectorSubtract ( wheel - > v , diff , wheel - > v ) ;
if ( wheel - > onGround ) {
// suspension bottomed out and tires are on the ground so we have
// to treat this as a collision of the frame with the ground
hitType = HTYPE_BOTTOMED_OUT ;
}
}
// length = car->springMinLength;
}
else if ( length > car - > springMaxLength ) {
if ( springVel > 0 ) {
// PM_ApplyPointBodyCollision(&car->sBody, &car->sPoints, car->sBody.up, 1.0);
/*
VectorScale ( car - > sBody . up , DotProduct ( car - > sBody . up , wheelVelocity ) , diff ) ;
VectorSubtract ( wheelVelocity , diff , delta ) ;
VectorScale ( car - > sBody . up , DotProduct ( car - > sBody . up , frameVelocity ) , diff ) ;
VectorAdd ( delta , diff , wheel - > v ) ;
*/
// move wheel so its not too far out
VectorMA ( wheel - > r , length - car - > springMaxLength , car - > sBody . up , wheel - > r ) ;
car - > sBody . curSpringLengths [ wheelIndex ] = car - > springMaxLength ;
}
hitType = HTYPE_MAXED_OUT ;
// length = car->springMaxLength;
}
// linear
// compression = car->springMaxLength - length;
/*
if ( length > car - > springMaxLength )
compression = car - > springMaxLength - length ;
else
compression = ( car - > springMaxLength * 2.0f - length ) / 5.0f ;
*/
// x^2
// compression = car->springMaxLength - length;
// compression *= compression;
// exp(x/max dist)
compression = car - > springMaxLength - length ;
if ( compression > 0 )
compression = ( car - > springMaxLength - car - > springMinLength ) * ( exp ( compression / ( car - > springMaxLength - car - > springMinLength ) ) - 1 ) / 1.718281828f ;
if ( compression < 0 )
VectorScale ( car - > sBody . up , 3.0f * CP_SPRING_STRENGTH * compression , frame - > forces [ SPRING ] ) ;
else
VectorScale ( car - > sBody . up , car - > springStrength * compression , frame - > forces [ SPRING ] ) ;
/*
{
float shock = VectorLength ( frame - > forces [ SHOCK ] ) ;
if ( shock > CP_MAX_SHOCK_FORCE )
{
# ifdef GAME
Com_Printf ( " %i Shock Force: %f \n " , ( wheel - & car - > sPoints [ 0 ] ) , shock ) ;
# endif
VectorScale ( frame - > forces [ SHOCK ] , CP_MAX_SHOCK_FORCE / shock , frame - > forces [ SHOCK ] ) ;
}
}
*/
//
// Calculate forces on the tire
//
VectorScale ( frame - > forces [ SPRING ] , - 1 , wheel - > forces [ SPRING ] ) ;
VectorScale ( frame - > forces [ SHOCK ] , - 1 , wheel - > forces [ SHOCK ] ) ;
return hitType ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_CalculateForces
Main force calculating function for the entire car and the wheels
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static void PM_CalculateForces ( car_t * car , carBody_t * body , carPoint_t * points , float sec ) {
vec3_t delta , diff ;
//vec3_t arm;
vec3_t force , hitOrigin , normal ;
float length , k , b , springVel , dot ;
int count ;
int i , hitType , n ;
2012-09-15 03:56:52 +00:00
//float impulseDamage;
2011-02-18 14:31:32 +00:00
VectorClear ( force ) ;
// add gravity forces
for ( i = 0 ; i < NUM_CAR_POINTS ; i + + ) {
VectorSet ( points [ i ] . forces [ GRAVITY ] , 0 , 0 , - CP_CURRENT_GRAVITY * points [ i ] . mass ) ;
}
VectorClear ( hitOrigin ) ;
count = 0 ;
PM_Generate_SwayBar_Forces ( car , body , points , 0 , 1 ) ;
PM_Generate_SwayBar_Forces ( car , body , points , 2 , 3 ) ;
// add spring forces
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + )
{
// VectorSubtract(points[i+4].r, points[i].r, delta);
if ( body - > curSpringLengths [ i ] > 20 )
{
// Com_Printf("Initialize wheel %i, frame %f,%f,%f, wheel %f,%f,%f\n", i, points[i+4].r[0], points[i+4].r[1], points[i+4].r[2], points[i].r[0], points[i].r[1], points[i].r[2]);
if ( i = = 0 )
PM_InitializeWheel ( body , points , i , 1.0f , - 1.0f ) ;
else if ( i = = 1 )
PM_InitializeWheel ( body , points , i , 1.0f , 1.0f ) ;
else if ( i = = 2 )
PM_InitializeWheel ( body , points , i , - 1.0f , - 1.0f ) ;
else if ( i = = 3 )
PM_InitializeWheel ( body , points , i , - 1.0f , 1.0f ) ;
}
hitType = PM_Generate_FrameWheel_Forces ( car , points , i + 4 , i ) ;
if ( VectorNAN ( points [ i ] . netForce ) )
Com_Printf ( " Blowing up car because of car point force on wheel %d \n " , i ) ;
if ( hitType = = HTYPE_BOTTOMED_OUT )
{
// add a normal to the frame point
for ( n = 0 ; n < 3 ; n + + )
{
if ( VectorLength ( points [ i + 4 ] . normals [ n ] ) ) continue ;
VectorCopy ( body - > up , points [ i + 4 ] . normals [ n ] ) ;
break ;
}
VectorAdd ( hitOrigin , points [ i + 4 ] . r , hitOrigin ) ;
count + + ;
}
else if ( hitType = = HTYPE_MAXED_OUT )
{
// add a normal to the frame point
for ( n = 0 ; n < 3 ; n + + )
{
if ( VectorLength ( points [ i + 4 ] . normals [ n ] ) ) continue ;
VectorScale ( body - > up , - 1.0f , points [ i + 4 ] . normals [ n ] ) ;
break ;
}
VectorAdd ( points [ i ] . forces [ SPRING ] , points [ i ] . forces [ SHOCK ] , force ) ;
dot = DotProduct ( force , body - > up ) ;
if ( dot < 0.0f )
VectorMA ( points [ i ] . forces [ SPRING ] , - dot , body - > up , points [ i ] . forces [ SPRING ] ) ;
}
}
if ( count ! = 0 )
{
if ( pm - > pDebug )
Com_Printf ( " PM_CalculateForces: Frame-Wheel Collision with %i wheels \n " , count ) ;
VectorScale ( hitOrigin , 1.0 / ( float ) count , hitOrigin ) ;
2012-09-15 03:56:52 +00:00
/*impulseDamage = */ PM_ApplyCollision ( body , points , hitOrigin , car - > sBody . up , body - > elasticity ) ;
2011-02-18 14:31:32 +00:00
// add normal force
// FIXME - change this so i can do it only once right before acceleration
// FIXME: replace with proper normal.netForce
/*
VectorSet ( delta , 0 , 0 , - CP_CURRENT_GRAVITY * CP_CAR_MASS ) ;
if ( DotProduct ( body - > up , delta ) < 0.0f ) {
VectorScale ( body - > up , - OVERCLIP * DotProduct ( body - > up , delta ) , body - > normalForce ) ;
PM_ApplyForce ( body , body - > normalForce , hitOrigin ) ;
}
*/
// damage stuff
// FIXME: taking too much damage during normal driving
/*
impulseDamage / = ( 2000.0f / sec ) / count ;
if ( impulseDamage > 0.00001f ) {
VectorCopy ( hitOrigin , pm - > damage . origin ) ;
VectorCopy ( car - > sBody . up , pm - > damage . dir ) ;
pm - > damage . damage + = impulseDamage ;
pm - > damage . mod = MOD_BO_SHOCKS ;
}
*/
VectorCopy ( hitOrigin , pm - > damage . origin ) ;
VectorCopy ( car - > sBody . up , pm - > damage . dir ) ;
pm - > damage . dflags = DAMAGE_NO_KNOCKBACK ;
pm - > damage . mod = MOD_BO_SHOCKS ;
}
// UPDATE - moved lower down
// PM_Generate_SwayBar_Forces(body, points, 0, 1);
// PM_Generate_SwayBar_Forces(body, points, 2, 3);
// calculate frame hits
VectorClear ( hitOrigin ) ;
VectorClear ( normal ) ;
count = 0 ;
for ( i = FIRST_FRAME_POINT ; i < NUM_CAR_POINTS ; i + + ) {
if ( ! points [ i ] . onGround ) continue ;
if ( DotProduct ( points [ i ] . v , points [ i ] . normals [ 0 ] ) > 0.0f ) continue ;
VectorAdd ( normal , points [ i ] . normals [ 0 ] , normal ) ;
VectorAdd ( hitOrigin , points [ i ] . r , hitOrigin ) ;
count + + ;
}
if ( count ! = 0 )
{
VectorScale ( hitOrigin , 1.0 / ( float ) count , hitOrigin ) ;
VectorNormalize ( normal ) ;
if ( pm - > pDebug )
Com_Printf ( " PM_CalculateForces: Frame hit a surface at %i spots \n " , count ) ;
2012-09-15 03:56:52 +00:00
/*impulseDamage = */ PM_ApplyCollision ( body , points , hitOrigin , normal , 0.0f ) ;
2011-02-18 14:31:32 +00:00
// add normal force
// FIXME - change this so i can do it only once right before acceleration
// FIXME: replace with proper normal.netForce
/*
VectorSet ( delta , 0 , 0 , - CP_CURRENT_GRAVITY * CP_CAR_MASS ) ;
if ( DotProduct ( normal , delta ) < 0.0f ) {
VectorScale ( body - > up , - OVERCLIP * DotProduct ( body - > up , delta ) , body - > normalForce ) ;
PM_ApplyForce ( body , body - > normalForce , hitOrigin ) ;
}
*/
// damage stuff - FIXME: dont take damage for sitting up against walls
// divide by number of spots hit maybe?
/*
impulseDamage / = ( 50000.0f / sec ) / count ;
if ( impulseDamage > 0.00001f ) {
VectorCopy ( hitOrigin , pm - > damage . origin ) ;
VectorCopy ( normal , pm - > damage . dir ) ;
pm - > damage . damage + = impulseDamage ;
pm - > damage . mod = MOD_WORLD_COLLISION ;
}
*/
VectorCopy ( hitOrigin , pm - > damage . origin ) ;
VectorCopy ( normal , pm - > damage . dir ) ;
pm - > damage . dflags = DAMAGE_NO_KNOCKBACK ;
pm - > damage . mod = MOD_WORLD_COLLISION ;
}
// steering, acceleration, braking, etc
PM_AddRoadForces ( car , body , points , sec ) ;
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + )
{
// new
// VectorMA(car->sPoints[i].r, -WHEEL_RADIUS, car->sPoints[i].normals[0], arm);
// remove component of force in body up direction
VectorCopy ( points [ i ] . forces [ ROAD ] , force ) ;
VectorMA ( force , - DotProduct ( force , car - > sBody . up ) , car - > sBody . up , force ) ;
// apply force to axle, not tire-ground contact
PM_ApplyForce ( body , force , car - > sPoints [ i ] . r ) ;
// PM_ApplyForce(body, force, arm);
// end new
VectorScale ( points [ i ] . forces [ ROAD ] , points [ i ] . mass / points [ i + 4 ] . mass , points [ i ] . forces [ ROAD ] ) ;
// }
// internal forces to return the wheel to perpendicular
// for (i = 0; i < FIRST_FRAME_POINT; i++)
// {
VectorSubtract ( points [ i + 4 ] . r , points [ i ] . r , delta ) ;
VectorMA ( delta , - DotProduct ( delta , car - > sBody . up ) , car - > sBody . up , diff ) ;
length = VectorNormalize ( diff ) ;
if ( length > 2.0f ) {
// k = 10000; // good at 90fps
k = pm - > car_wheel * CP_WHEEL_MASS ;
// b = 2 * sqrt(k * points[i].mass);
b = pm - > car_wheel_damp * CP_WHEEL_MASS ;
VectorScale ( diff , - k * length , force ) ;
VectorSubtract ( points [ i + 4 ] . v , points [ i ] . v , delta ) ;
VectorMA ( delta , - DotProduct ( delta , car - > sBody . up ) , car - > sBody . up , diff ) ;
springVel = VectorNormalize ( diff ) ;
VectorMA ( force , - b * springVel , diff , force ) ;
// VectorCopy(force, points[i+4].forces[INTERNAL]);
VectorScale ( force , - 1 , points [ i ] . forces [ INTERNAL ] ) ;
}
}
// calculate net forces
for ( i = 0 ; i < NUM_CAR_POINTS ; i + + ) {
PM_CalculateNetForce ( & points [ i ] , i ) ;
}
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_AccelerateAndMoveBody
This function is largely based on the physics articles and
source from Chris Hecker : http : //www.d6.com/users/checker/dynamics.htm
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static void PM_AccelerateAndMoveBody ( car_t * car , carBody_t * sBody , carBody_t * tBody , float time ) {
float m [ 3 ] [ 3 ] ;
float m2 [ 3 ] [ 3 ] ;
vec3_t vec ;
vec3_t force ;
int i , n ;
float dot , impulseDamage ;
float time_2 = time / 2.0f ;
// FIXME: handle collisions here as well?
// FIXME: just have one normal force for the entire body instead of for each point?
for ( i = FIRST_FRAME_POINT ; i < NUM_CAR_POINTS ; i + + ) {
if ( ! car - > sPoints [ i ] . onGround ) continue ;
for ( n = 0 ; n < 3 ; n + + ) {
if ( ! VectorLength ( car - > sPoints [ i ] . normals [ n ] ) ) continue ;
dot = DotProduct ( car - > sPoints [ i ] . normals [ n ] , sBody - > netForce ) ;
if ( dot > = 0.0f ) continue ;
VectorScale ( car - > sPoints [ i ] . normals [ n ] , - OVERCLIP * dot , force ) ;
PM_ApplyForce ( sBody , force , car - > sPoints [ i ] . r ) ;
}
}
// damage stuff
VectorSubtract ( tBody - > v , sBody - > v , vec ) ;
impulseDamage = VectorLength ( vec ) ;
if ( impulseDamage > 5.0f ) {
// Com_Printf("impulseDamage %f\n", impulseDamage);
pm - > damage . damage + = impulseDamage / 25.0f ;
}
// linear
// NOTE: this method is really not right but it works so oh well.
// tBody contains netForce from last frame
VectorAdd ( sBody - > netForce , tBody - > netForce , vec ) ;
VectorMA ( sBody - > v , time_2 / sBody - > mass , vec , tBody - > v ) ;
VectorAdd ( sBody - > v , tBody - > v , vec ) ;
VectorMA ( sBody - > r , time_2 , vec , tBody - > r ) ;
// angular
VectorAdd ( sBody - > netMoment , tBody - > netMoment , vec ) ;
VectorMA ( sBody - > L , time_2 , vec , tBody - > L ) ;
VectorRotate ( tBody - > L , sBody - > inverseWorldInertiaTensor , tBody - > w ) ;
VectorAdd ( sBody - > w , tBody - > w , vec ) ;
m [ 0 ] [ 0 ] = 0 ; m [ 0 ] [ 1 ] = time_2 * - vec [ 2 ] ; m [ 0 ] [ 2 ] = time_2 * vec [ 1 ] ;
m [ 1 ] [ 0 ] = time_2 * vec [ 2 ] ; m [ 1 ] [ 1 ] = 0 ; m [ 1 ] [ 2 ] = time_2 * - vec [ 0 ] ;
m [ 2 ] [ 0 ] = time_2 * - vec [ 1 ] ; m [ 2 ] [ 1 ] = time_2 * vec [ 0 ] ; m [ 2 ] [ 2 ] = 0 ;
MatrixMultiply ( m , sBody - > t , m2 ) ;
MatrixAdd ( sBody - > t , m2 , tBody - > t ) ;
OrthonormalizeOrientation ( tBody - > t ) ;
// Optimization note: check if inverseWorldInertiaTensor is only really
// needed for collisions. If it is then might be best just to calculate
// it there since collisions dont usually happen every frame.
// Note: only used in collisions and converting L to w. If I can get w
// some other way then I can move it.
// MatrixMultiply(tBody->t, car->inverseBodyInertiaTensor, m);
// This should be the same as MatrixMultiply(tBody->t, car->inverseBodyInertiaTensor, m);
// without so many multiplies and adds.
m [ 0 ] [ 0 ] = car - > inverseBodyInertiaTensor [ 0 ] [ 0 ] * tBody - > t [ 0 ] [ 0 ] ;
m [ 1 ] [ 0 ] = car - > inverseBodyInertiaTensor [ 0 ] [ 0 ] * tBody - > t [ 1 ] [ 0 ] ;
m [ 2 ] [ 0 ] = car - > inverseBodyInertiaTensor [ 0 ] [ 0 ] * tBody - > t [ 2 ] [ 0 ] ;
m [ 0 ] [ 1 ] = car - > inverseBodyInertiaTensor [ 1 ] [ 1 ] * tBody - > t [ 0 ] [ 1 ] ;
m [ 1 ] [ 1 ] = car - > inverseBodyInertiaTensor [ 1 ] [ 1 ] * tBody - > t [ 1 ] [ 1 ] ;
m [ 2 ] [ 1 ] = car - > inverseBodyInertiaTensor [ 1 ] [ 1 ] * tBody - > t [ 2 ] [ 1 ] ;
m [ 0 ] [ 2 ] = car - > inverseBodyInertiaTensor [ 2 ] [ 2 ] * tBody - > t [ 0 ] [ 2 ] ;
m [ 1 ] [ 2 ] = car - > inverseBodyInertiaTensor [ 2 ] [ 2 ] * tBody - > t [ 1 ] [ 2 ] ;
m [ 2 ] [ 2 ] = car - > inverseBodyInertiaTensor [ 2 ] [ 2 ] * tBody - > t [ 2 ] [ 2 ] ;
MatrixTranspose ( tBody - > t , m2 ) ;
MatrixMultiply ( m , m2 , tBody - > inverseWorldInertiaTensor ) ;
// generate direction vectors
OrientationToVectors ( tBody - > t , tBody - > forward , tBody - > right , tBody - > up ) ;
// copy netForce and netMoment to target so we can use it next frame
VectorCopy ( sBody - > netForce , tBody - > netForce ) ;
VectorCopy ( sBody - > netMoment , tBody - > netMoment ) ;
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_CalculateSecondaryQuantities
Used in cgame to calculate w and forward , right , up from the new known
primary values from the server
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
void PM_CalculateSecondaryQuantities ( car_t * car , carBody_t * body , carPoint_t * points ) {
float m [ 3 ] [ 3 ] ;
float m2 [ 3 ] [ 3 ] ;
float forwardScale , rightScale , upScale ;
vec3_t diff ;
int i ;
// MatrixMultiply(body->t, car->inverseBodyInertiaTensor, m);
m [ 0 ] [ 0 ] = car - > inverseBodyInertiaTensor [ 0 ] [ 0 ] * body - > t [ 0 ] [ 0 ] ;
m [ 1 ] [ 0 ] = car - > inverseBodyInertiaTensor [ 0 ] [ 0 ] * body - > t [ 1 ] [ 0 ] ;
m [ 2 ] [ 0 ] = car - > inverseBodyInertiaTensor [ 0 ] [ 0 ] * body - > t [ 2 ] [ 0 ] ;
m [ 0 ] [ 1 ] = car - > inverseBodyInertiaTensor [ 1 ] [ 1 ] * body - > t [ 0 ] [ 1 ] ;
m [ 1 ] [ 1 ] = car - > inverseBodyInertiaTensor [ 1 ] [ 1 ] * body - > t [ 1 ] [ 1 ] ;
m [ 2 ] [ 1 ] = car - > inverseBodyInertiaTensor [ 1 ] [ 1 ] * body - > t [ 2 ] [ 1 ] ;
m [ 0 ] [ 2 ] = car - > inverseBodyInertiaTensor [ 2 ] [ 2 ] * body - > t [ 0 ] [ 2 ] ;
m [ 1 ] [ 2 ] = car - > inverseBodyInertiaTensor [ 2 ] [ 2 ] * body - > t [ 1 ] [ 2 ] ;
m [ 2 ] [ 2 ] = car - > inverseBodyInertiaTensor [ 2 ] [ 2 ] * body - > t [ 2 ] [ 2 ] ;
MatrixTranspose ( body - > t , m2 ) ;
MatrixMultiply ( m , m2 , body - > inverseWorldInertiaTensor ) ;
VectorRotate ( body - > L , body - > inverseWorldInertiaTensor , body - > w ) ;
OrientationToVectors ( body - > t , body - > forward , body - > right , body - > up ) ;
// set locations and velocities of frame points
PM_InitializeFrame ( body , & points [ FL_FRAME ] , 1.0f , - 1.0f , 0.0f ) ;
PM_InitializeFrame ( body , & points [ FR_FRAME ] , 1.0f , 1.0f , 0.0f ) ;
PM_InitializeFrame ( body , & points [ RL_FRAME ] , - 1.0f , - 1.0f , 0.0f ) ;
PM_InitializeFrame ( body , & points [ RR_FRAME ] , - 1.0f , 1.0f , 0.0f ) ;
// body collision points
forwardScale = ( ( CAR_LENGTH / 2.0f ) - BODY_RADIUS ) / WHEEL_FORWARD ;
rightScale = ( ( CAR_WIDTH / 2.0f ) - BODY_RADIUS ) / WHEEL_RIGHT ;
upScale = ( 0 ) / - WHEEL_UP ;
PM_InitializeFrame ( body , & points [ FL_BODY ] , forwardScale , - rightScale , upScale ) ;
PM_InitializeFrame ( body , & points [ FR_BODY ] , forwardScale , rightScale , upScale ) ;
PM_InitializeFrame ( body , & points [ ML_BODY ] , 0.0f , - rightScale , upScale ) ;
PM_InitializeFrame ( body , & points [ MR_BODY ] , 0.0f , rightScale , upScale ) ;
PM_InitializeFrame ( body , & points [ RL_BODY ] , - forwardScale , - rightScale , upScale ) ;
PM_InitializeFrame ( body , & points [ RR_BODY ] , - forwardScale , rightScale , upScale ) ;
PM_InitializeFrame ( body , & points [ ML_ROOF ] , 0.0f , - rightScale , 0.6f ) ;
PM_InitializeFrame ( body , & points [ MR_ROOF ] , 0.0f , rightScale , 0.6f ) ;
// calculate spring lengths
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + )
{
VectorSubtract ( points [ i + 4 ] . r , points [ i ] . r , diff ) ;
body - > curSpringLengths [ i ] = DotProduct ( diff , body - > up ) ;
}
PM_SetCoM ( body , points ) ;
PM_ClearCarForces ( body , points ) ;
PM_CopyTargetToSource ( & car - > sBody , & car - > tBody , car - > sPoints , car - > tPoints ) ;
/*
VectorCopy ( car - > sBody . v , car - > oldBodies [ 2 ] . v ) ;
VectorCopy ( car - > sBody . w , car - > oldBodies [ 2 ] . w ) ;
VectorCopy ( car - > sBody . netForce , car - > oldBodies [ 2 ] . netForce ) ;
VectorCopy ( car - > sBody . netMoment , car - > oldBodies [ 2 ] . netMoment ) ;
VectorCopy ( car - > sBody . v , car - > oldBodies [ 1 ] . v ) ;
VectorCopy ( car - > sBody . w , car - > oldBodies [ 1 ] . w ) ;
VectorCopy ( car - > sBody . netForce , car - > oldBodies [ 1 ] . netForce ) ;
VectorCopy ( car - > sBody . netMoment , car - > oldBodies [ 1 ] . netMoment ) ;
VectorCopy ( car - > sBody . v , car - > oldBodies [ 0 ] . v ) ;
VectorCopy ( car - > sBody . w , car - > oldBodies [ 0 ] . w ) ;
VectorCopy ( car - > sBody . netForce , car - > oldBodies [ 0 ] . netForce ) ;
VectorCopy ( car - > sBody . netMoment , car - > oldBodies [ 0 ] . netMoment ) ;
for ( i = 0 ; i < LAST_RW_POINT ; i + + )
{
VectorCopy ( car - > sPoints [ i ] . netForce , car - > oldPoints [ 2 ] [ i ] . netForce ) ;
VectorCopy ( car - > sPoints [ i ] . v , car - > oldPoints [ 2 ] [ i ] . v ) ;
car - > oldPoints [ 2 ] [ i ] . netMoment = car - > sPoints [ i ] . netMoment ;
VectorCopy ( car - > sPoints [ i ] . netForce , car - > oldPoints [ 1 ] [ i ] . netForce ) ;
VectorCopy ( car - > sPoints [ i ] . v , car - > oldPoints [ 1 ] [ i ] . v ) ;
car - > oldPoints [ 1 ] [ i ] . netMoment = car - > sPoints [ i ] . netMoment ;
VectorCopy ( car - > sPoints [ i ] . netForce , car - > oldPoints [ 0 ] [ i ] . netForce ) ;
VectorCopy ( car - > sPoints [ i ] . v , car - > oldPoints [ 0 ] [ i ] . v ) ;
car - > oldPoints [ 0 ] [ i ] . netMoment = car - > sPoints [ i ] . netMoment ;
}
*/
}
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_CalculateTargetBody
Integrates the current state to find the next state .
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
static void PM_CalculateTargetBody ( car_t * car , carBody_t * sBody , carBody_t * tBody , carPoint_t * sPoints , carPoint_t * tPoints , float time ) {
int i ;
float forwardScale , rightScale , upScale ;
vec3_t diff ;
// move body
PM_AccelerateAndMoveBody ( car , sBody , tBody , time ) ;
// move wheels
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + ) {
PM_AccelerateAndMove ( car , & sPoints [ i ] , & tPoints [ i ] , time ) ;
}
// set locations and velocities of frame points
PM_InitializeFrame ( tBody , & tPoints [ FL_FRAME ] , 1.0f , - 1.0f , 0.0f ) ;
PM_InitializeFrame ( tBody , & tPoints [ FR_FRAME ] , 1.0f , 1.0f , 0.0f ) ;
PM_InitializeFrame ( tBody , & tPoints [ RL_FRAME ] , - 1.0f , - 1.0f , 0.0f ) ;
PM_InitializeFrame ( tBody , & tPoints [ RR_FRAME ] , - 1.0f , 1.0f , 0.0f ) ;
// body collision points
forwardScale = ( ( CAR_LENGTH / 2.0f ) - BODY_RADIUS ) / WHEEL_FORWARD ;
rightScale = ( ( CAR_WIDTH / 2.0f ) - BODY_RADIUS ) / WHEEL_RIGHT ;
upScale = ( 0 ) / - WHEEL_UP ;
PM_InitializeFrame ( tBody , & tPoints [ FL_BODY ] , forwardScale , - rightScale , upScale ) ;
PM_InitializeFrame ( tBody , & tPoints [ FR_BODY ] , forwardScale , rightScale , upScale ) ;
PM_InitializeFrame ( tBody , & tPoints [ ML_BODY ] , 0.0f , - rightScale , upScale ) ;
PM_InitializeFrame ( tBody , & tPoints [ MR_BODY ] , 0.0f , rightScale , upScale ) ;
PM_InitializeFrame ( tBody , & tPoints [ RL_BODY ] , - forwardScale , - rightScale , upScale ) ;
PM_InitializeFrame ( tBody , & tPoints [ RR_BODY ] , - forwardScale , rightScale , upScale ) ;
PM_InitializeFrame ( tBody , & tPoints [ ML_ROOF ] , 0.0f , - rightScale , 0.6f ) ;
PM_InitializeFrame ( tBody , & tPoints [ MR_ROOF ] , 0.0f , rightScale , 0.6f ) ;
// calculate spring lengths
for ( i = 0 ; i < FIRST_FRAME_POINT ; i + + )
{
VectorSubtract ( tPoints [ i + 4 ] . r , tPoints [ i ] . r , diff ) ;
tBody - > curSpringLengths [ i ] = DotProduct ( diff , tBody - > up ) ;
}
// set the center of mass
PM_SetCoM ( tBody , tPoints ) ;
}
/*
qboolean PM_Hit_Car ( trace_t trace ) {
// hit another car
vec3_t f , r , u , delta ;
float dotF , dotR , dotU ;
qboolean lengthHitOK , widthHitOK , heightHitOK ;
lengthHitOK = qfalse ;
widthHitOK = qfalse ;
heightHitOK = qfalse ;
VectorSubtract ( trace . endpos , pm - > cars [ trace . entityNum ] - > sBody . r , delta ) ;
OrientationToVectors ( pm - > cars [ trace . entityNum ] - > sBody . t , f , r , u ) ;
// need to add radius of trace box to this check?
dotF = DotProduct ( delta , f ) ;
dotR = DotProduct ( delta , r ) ;
dotU = DotProduct ( delta , u ) ;
if ( dotU < CAR_HEIGHT / 2 + WHEEL_RADIUS & & dotU > - CAR_HEIGHT / 2 - WHEEL_RADIUS ) {
heightHitOK = qtrue ;
}
if ( dotF < CAR_LENGTH / 2 + WHEEL_RADIUS & & dotF > - CAR_LENGTH / 2 - WHEEL_RADIUS ) {
lengthHitOK = qtrue ;
}
if ( dotR < CAR_WIDTH / 2 + WHEEL_RADIUS & & dotR > - CAR_WIDTH / 2 - WHEEL_RADIUS ) {
widthHitOK = qtrue ;
}
// if (heightHitOK){
# ifdef GAME
// G_LogPrintf( "%d hit %d\n", pm->ps->clientNum, trace.entityNum);
// G_LogPrintf( "trace.endpos : %f, %f, %f\n", trace.endpos[0], trace.endpos[1], trace.endpos[2]);
// G_LogPrintf( "pm->cars[trace.entityNum]->sBody.r : %f, %f, %f\n", pm->cars[trace.entityNum]->sBody.r[0], pm->cars[trace.entityNum]->sBody.r[1], pm->cars[trace.entityNum]->sBody.r[2]);
// G_LogPrintf( "delta : %f, %f, %f\n", delta[0], delta[1], delta[2]);
// G_LogPrintf( "height dist: %f, max %f\n", dotU, CAR_HEIGHT/2 + WHEEL_RADIUS);
// G_LogPrintf( "length dist: %f, max %f\n", dotF, CAR_LENGTH/2 + WHEEL_RADIUS);
// G_LogPrintf( "width dist: %f, max %f\n", dotR, CAR_WIDTH/2 + WHEEL_RADIUS);
// G_LogPrintf( "f : %f, %f, %f\n", f[0], f[1], f[2]);
// G_LogPrintf( "r : %f, %f, %f\n", r[0], r[1], r[2]);
// G_LogPrintf( "u : %f, %f, %f\n", u[0], u[1], u[2]);
# endif
// }
if ( widthHitOK ) {
# ifdef GAME
G_LogPrintf ( " width is ok \n " ) ;
# endif
return qtrue ;
}
if ( lengthHitOK & & heightHitOK & & widthHitOK ) {
# ifdef GAME
G_LogPrintf ( " all dimensions are ok \n " ) ;
# endif
return qtrue ;
}
return qfalse ;
}
*/
/*
= = = = = = = = = = = = = = = = = = =
PM_Trace_Points
Traces out the points from current state to next state to check for collisions .
Partially based on the PM_SlideMove function .
= = = = = = = = = = = = = = = = = = =
*/
# define MAX_CLIP_PLANES 3
static void PM_Trace_Points ( car_t * car , carPoint_t * sPoints , carPoint_t * tPoints , float time ) {
vec3_t maxs , mins ;
vec3_t start , dest , dir ;
//vec3_t normal;
trace_t trace ;
int i , j ;
//float minTrace = 1.0f;
//int hitFirst, hitEnt;
carPoint_t * sPoint , * tPoint ;
//int count = 0;
vec3_t hitOrigin ;
// new stuff
int bumpcount , numbumps ;
float d ;
int numplanes ;
vec3_t vel ;
vec3_t clipVelocity ;
int k ;
//int l;
float time_left ;
float into ;
numbumps = 4 ;
// end
VectorClear ( hitOrigin ) ;
// should actually do all points
for ( i = 0 ; i < NUM_CAR_POINTS ; i + + ) {
// skip collision detection of suspension points
if ( i > = FIRST_FRAME_POINT & & i < LAST_FRAME_POINT ) continue ;
sPoint = & sPoints [ i ] ;
tPoint = & tPoints [ i ] ;
VectorSet ( mins , - sPoint - > radius , - sPoint - > radius , - sPoint - > radius ) ;
VectorSet ( maxs , sPoint - > radius , sPoint - > radius , sPoint - > radius ) ;
if ( i > = LAST_FRAME_POINT ) {
mins [ 2 ] / = 1.5f ;
maxs [ 2 ] / = 1.5f ;
}
/*
VectorSubtract ( car - > sBody . r , sPoints [ i ] . r , dir ) ;
if ( VectorLength ( dir ) )
VectorScale ( dir , 20.0f / VectorLength ( dir ) , dir ) ;
VectorCopy ( sPoints [ i ] . r , start ) ;
VectorAdd ( start , dir , start ) ;
VectorCopy ( tPoints [ i ] . r , dest ) ;
// trace the frame to target position
pm - > trace ( & trace , start , mins , maxs , dest , pm - > ps - > clientNum , pm - > tracemask ) ;
# ifdef GAME
if ( trace . contents & CONTENTS_BODY ) {
// G_LogPrintf("car was hit\n");
if ( trace . startsolid )
Com_Printf ( " start solid and hit CONTENTS_BODY \n " ) ;
if ( trace . allsolid )
Com_Printf ( " all solid and hit CONTENTS_BODY \n " ) ;
// need to make it at least a tiny ways
if ( trace . fraction ! = 0.0f ) {
if ( trace . fraction < minTrace ) {
minTrace = trace . fraction ;
VectorClear ( hitOrigin ) ;
count = 0 ;
}
if ( trace . fraction = = minTrace ) {
VectorAdd ( hitOrigin , trace . endpos , hitOrigin ) ;
count + + ;
if ( g_entities [ trace . entityNum ] . flags & FL_EXTRA_BBOX )
hitEnt = g_entities [ trace . entityNum ] . r . ownerNum ;
else
hitEnt = trace . entityNum ;
// PM_CheckSurfaceFlags( &trace, &tPoints[i] );
}
// PM_SetFluidDensity(tPoints, i);
// continue;
}
if ( trace . fraction = = 0 )
Com_Printf ( " fraction == 0 and hit CONTENTS_BODY \n " ) ;
// trace the frame to target position but skip other cars
pm - > trace ( & trace , start , mins , maxs , dest , pm - > ps - > clientNum , pm - > tracemask & ~ CONTENTS_BODY ) ;
}
VectorClear ( tPoints [ i ] . normals [ 1 ] ) ;
# endif
if ( trace . contents & CONTENTS_BODY ) {
Com_Printf ( " hit body again? \n " ) ;
}
// if the frame is flush on the ground
if ( trace . fraction = = 0.0F ) {
tPoints [ i ] . onGround = qtrue ;
if ( VectorLength ( trace . plane . normal ) > 0.0F ) {
VectorCopy ( trace . plane . normal , tPoints [ i ] . normals [ 0 ] ) ;
}
PM_CheckSurfaceFlags ( & trace , & tPoints [ i ] ) ;
} else if ( trace . fraction < 1.0F ) {
tPoints [ i ] . onGround = qtrue ;
VectorCopy ( trace . plane . normal , tPoints [ i ] . normals [ 0 ] ) ;
PM_CheckSurfaceFlags ( & trace , & tPoints [ i ] ) ;
} else {
tPoints [ i ] . onGround = qfalse ;
continue ;
}
*/
// move the point in towards the center of the car
// this is to try to stop the wheel bboxes from sitting directly on the surface
// and then not getting a valid trace because it is startsoild
VectorSubtract ( car - > sBody . r , sPoint - > r , dir ) ;
if ( VectorLengthSquared ( dir ) )
VectorScale ( dir , 20.0f / VectorLength ( dir ) , dir ) ;
VectorAdd ( sPoint - > r , dir , start ) ;
VectorCopy ( tPoint - > r , dest ) ;
/*
numTraces + + ;
pm - > trace ( & trace , sPoints [ i ] . r , mins , maxs , sPoints [ i ] . r , pm - > ps - > clientNum , pm - > tracemask ) ;
if ( trace . startsolid )
{
// if we are starting solid then offset the starting point otherwise
// we dont need to.
Com_Printf ( " the start is solid! \n " ) ;
// try adding on a non zero velocity and the previous normal vector
// if( VectorLengthSquared( sPoints[i].v ) == 0.0f )
// VectorMA( sPoints[i].lastNonZeroVelocity, -1.0f, sPoints[i].normals[0], dir );
// else
VectorMA ( sPoints [ i ] . v , - 20.0f , sPoints [ i ] . normals [ 0 ] , dir ) ;
VectorNormalize ( dir ) ;
VectorSubtract ( sPoints [ i ] . r , dir , start ) ;
}
else
VectorCopy ( sPoints [ i ] . r , start ) ;
VectorCopy ( tPoints [ i ] . r , dest ) ;
*/
// OPTIMIZE: just use sPoints[i].v if not startsolid?
// VectorSubtract( dest, start, vel );
VectorSubtract ( tPoint - > r , start , vel ) ;
VectorScale ( vel , 1 / time , vel ) ;
numplanes = 0 ;
time_left = time ;
for ( bumpcount = 0 ; bumpcount < numbumps ; bumpcount + + ) {
VectorMA ( start , time_left , vel , dest ) ;
// see if we can make it there
numTraces + + ;
pm - > trace ( & trace , start , mins , maxs , dest , pm - > ps - > clientNum , pm - > tracemask ) ;
if ( trace . startsolid & & ! bumpcount ) {
VectorCopy ( sPoint - > r , start ) ;
VectorSubtract ( dest , start , vel ) ;
VectorScale ( vel , 1 / time , vel ) ;
// Com_Printf("Start point %d is in a solid\n", i);
continue ;
}
if ( trace . allsolid ) {
// entity is completely trapped in another solid
// Com_Printf("trace %d is all solid\n", i);
break ;
}
if ( trace . fraction > 0 ) {
// actually covered some distance
// VectorCopy ( trace.endpos, start );
VectorScale ( trace . endpos , 0.999f , start ) ;
}
if ( trace . fraction = = 1 ) {
if ( ! bumpcount ) {
tPoint - > onGround = qfalse ;
VectorClear ( tPoint - > normals [ 0 ] ) ;
VectorClear ( tPoint - > normals [ 1 ] ) ;
VectorClear ( tPoint - > normals [ 2 ] ) ;
}
break ; // moved the entire distance
}
time_left - = time_left * trace . fraction ;
if ( numplanes > = MAX_CLIP_PLANES ) {
// this shouldn't really happen
// Com_Printf("numplanes >= MAX_CLIP_PLANES\n");
break ;
}
//
// if this is the same plane we hit before, nudge velocity
// out along it, which fixes some epsilon issues with
// non-axial planes
//
for ( j = 0 ; j < numplanes ; j + + ) {
if ( DotProduct ( trace . plane . normal , tPoint - > normals [ j ] ) > 0.99 ) {
VectorAdd ( trace . plane . normal , vel , vel ) ;
break ;
}
}
if ( j < numplanes ) {
continue ;
}
# ifdef GAME
if ( trace . contents & CONTENTS_BODY ) {
if ( g_entities [ trace . entityNum ] . flags & FL_EXTRA_BBOX ) {
hitFirst = g_entities [ trace . entityNum ] . r . ownerNum ;
// Com_Printf("Hit extra bbox\n");
}
else
hitFirst = trace . entityNum ;
if ( g_entities [ hitFirst ] . client ) {
// G_LogPrintf("car was hit\n");
// if (trace.startsolid)
// Com_Printf("start solid and hit CONTENTS_BODY\n");
// if (trace.allsolid)
// Com_Printf("all solid and hit CONTENTS_BODY\n");
// need to make it at least a tiny ways
if ( trace . fraction ! = 0.0f )
{
if ( trace . fraction < minTrace )
{
minTrace = trace . fraction ;
VectorClear ( hitOrigin ) ;
count = 0 ;
}
if ( trace . fraction = = minTrace )
{
VectorAdd ( hitOrigin , trace . endpos , hitOrigin ) ;
count + + ;
hitEnt = hitFirst ;
tPoints [ i ] . onGround = qtrue ;
// PM_CheckSurfaceFlags( &trace, &tPoints[i] );
}
// PM_SetFluidDensity(tPoints, i);
// continue;
}
else
{
2012-09-15 03:56:52 +00:00
// inside another car
2011-02-18 14:31:32 +00:00
// Com_Printf("fraction == 0 and hit CONTENTS_BODY\n");
}
// trace the frame to target position but skip other cars
// pm->trace( &trace, start, mins, maxs, dest, pm->ps->clientNum, pm->tracemask & ~CONTENTS_BODY );
pm - > tracemask & = ~ CONTENTS_BODY ;
continue ;
}
else {
// Com_Printf( "hit non player CONTENTS_BODY\n" );
PM_AddTouchEnt ( trace . entityNum , trace . endpos ) ;
pm - > tracemask & = ~ CONTENTS_BODY ;
continue ;
}
}
# endif
VectorCopy ( trace . plane . normal , tPoint - > normals [ numplanes ] ) ;
tPoint - > onGround = qtrue ;
PM_CheckSurfaceFlags ( & trace , tPoint ) ;
numplanes + + ;
//
// modify velocity so it parallels all of the clip planes
//
// find a plane that it enters
for ( j = 0 ; j < numplanes ; j + + ) {
into = DotProduct ( vel , tPoint - > normals [ j ] ) ;
if ( into > 0.01f ) {
continue ; // move doesn't interact with the plane
}
// see how hard we are hitting things
// if ( -into > pml.impactSpeed ) {
// pml.impactSpeed = -into;
// }
// slide along the plane
VectorMA ( vel , - into * OVERCLIP , tPoint - > normals [ j ] , clipVelocity ) ;
// PM_ClipVelocity ( vel, tPoints[i].normals[j], clipVelocity, OVERCLIP );
// see if there is a second plane that the new move enters
for ( k = 0 ; k < numplanes ; k + + ) {
if ( k = = j ) {
continue ;
}
into = DotProduct ( clipVelocity , tPoint - > normals [ k ] ) ;
if ( into > 0.01f ) {
continue ; // move doesn't interact with the plane
}
// try clipping the move to the plane
VectorMA ( clipVelocity , - into * OVERCLIP , tPoint - > normals [ j ] , clipVelocity ) ;
// PM_ClipVelocity( clipVelocity, tPoints[i].normals[k], clipVelocity, OVERCLIP );
// see if it goes back into the first clip plane
if ( DotProduct ( clipVelocity , tPoint - > normals [ j ] ) > = 0 ) {
continue ;
}
// slide the original velocity along the crease
CrossProduct ( tPoint - > normals [ j ] , tPoint - > normals [ k ] , dir ) ;
VectorNormalize ( dir ) ;
d = DotProduct ( dir , vel ) ;
VectorScale ( dir , d , clipVelocity ) ;
// dont do this because i never see that triple plane interaction message
// so it probably only rarely happens, if ever
/*
// see if there is a third plane the the new move enters
for ( l = 0 ; l < numplanes ; l + + ) {
if ( l = = j | | l = = k ) {
continue ;
}
if ( DotProduct ( clipVelocity , tPoints [ i ] . normals [ l ] ) > = 0.1 ) {
continue ; // move doesn't interact with the plane
}
// stop dead at a tripple plane interaction
Com_Printf ( " triple plane interaction \n " ) ;
return ;
}
*/
}
// if we have fixed all interactions, try another move
VectorCopy ( clipVelocity , vel ) ;
// VectorCopy( endClipVelocity, endVelocity );
break ;
}
}
// NEW- copy final position to the point
// VectorCopy( start, tPoints[i].r );
PM_SetFluidDensity ( tPoints , i ) ;
}
# ifdef GAME
if ( count & & pm - > cars [ hitEnt ] )
{
float impulseDamage ;
// G_LogPrintf( "minTrace %f\n", minTrace );
// G_LogPrintf("count = %d\n", count);
// G_LogPrintf("pml.physicsSplit = %d\n", pml.physicsSplit);
// calculate car position at collision
// G_LogPrintf( "car was hit with %f traced\n", minTrace );
VectorScale ( hitOrigin , 1.0f / count , hitOrigin ) ;
// G_LogPrintf("hitOrigin = %f, %f, %f\n", hitOrigin[0], hitOrigin[1], hitOrigin[2]);
PM_CalculateTargetBody ( car , & car - > sBody , & car - > tBody , car - > sPoints , car - > tPoints , time * minTrace ) ;
// VectorSubtract(car->tBody.r, pm->cars[hitEnt]->sBody.r, normal);
VectorSubtract ( hitOrigin , pm - > cars [ hitEnt ] - > sBody . r , normal ) ;
VectorNormalize ( normal ) ;
impulseDamage = PM_ApplyBodyBodyCollision ( & car - > tBody , car - > tPoints , & pm - > cars [ hitEnt ] - > sBody , pm - > cars [ hitEnt ] - > sPoints , hitOrigin , normal , 0.25f ) ;
// set normal on this car
for ( i = 0 ; i < 3 ; i + + ) {
if ( ! VectorLength ( tPoints [ hitEnt ] . normals [ i ] ) ) {
VectorScale ( normal , - 1.0f , tPoints [ hitEnt ] . normals [ i ] ) ;
break ;
}
}
// damage stuff
VectorCopy ( hitOrigin , pm - > damage . origin ) ;
VectorCopy ( normal , pm - > damage . dir ) ;
pm - > damage . dflags = DAMAGE_NO_KNOCKBACK ;
pm - > damage . mod = MOD_CAR_COLLISION ;
pm - > damage . otherEnt = trace . entityNum ;
PM_CopyTargetToSource ( & car - > tBody , & car - > sBody , tPoints , sPoints ) ;
// run physics again but remove CONTENTS_BODY first so it cant collide with cars
pml . physicsSplit + + ;
pm - > tracemask & = ~ CONTENTS_BODY ;
PM_DriveMove ( car , time * ( 1.0f - minTrace ) , qfalse ) ;
}
# endif
// break the frame up into parts
// fix this so it doesnt cause infinite loops
/*
if ( minTrace < 1.0f ) {
// Check for touching multiple surfaces with different normals
// (wheel on ground and against wall)
VectorCopy ( sPoints [ hitFirst ] . r , start ) ;
VectorCopy ( tPoints [ hitFirst ] . r , dest ) ;
VectorSubtract ( dest , start , dir ) ;
length = VectorNormalize ( dir ) ;
VectorMA ( start , ( length * minTrace ) - 0.05f , dir , start ) ;
VectorClear ( normal ) ;
for ( i = 0 ; i < 3 ; i + + ) {
VectorCopy ( start , dest ) ;
dest [ i ] + = dir [ i ] > 0.0f ? 0.10f : - 0.10f ;
pm - > trace ( & trace , start , mins , maxs , dest , pm - > ps - > clientNum , pm - > tracemask ) ;
if ( trace . fraction < 1.0F ) {
uniqueNormal = qtrue ;
for ( j = 0 ; j < 3 ; j + + ) {
if ( i = = j ) continue ;
if ( VectorCompare ( trace . plane . normal , tPoints [ hitFirst ] . normals [ j ] ) ) {
uniqueNormal = qfalse ;
break ;
}
}
if ( uniqueNormal & & VectorLength ( trace . plane . normal ) ) {
VectorCopy ( trace . plane . normal , tPoints [ hitFirst ] . normals [ i ] ) ;
}
}
}
if ( pml . physicsSplit > 5 ) {
// Com_Printf("Breaking out of physics loop\n");
}
else {
pml . physicsSplit + + ;
PM_CalculateTargetBody ( car , & car - > sBody , & car - > tBody , car - > sPoints , car - > tPoints , time * minTrace ) ;
PM_CopyTargetToSource ( & car - > tBody , & car - > sBody , car - > tPoints , car - > sPoints ) ;
PM_DriveMove ( car , time * ( 1.0f - minTrace ) ) ;
}
// }
}
*/
}
//int trap_Milliseconds( void );
/*
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
PM_DriveMove
The mother functioner of Q3Rally car movement . Calculates the forces and torques
2012-09-15 03:56:52 +00:00
on the car , integrates to get the next state and traces to handle collisions with
2011-02-18 14:31:32 +00:00
the world and other objects .
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
*/
void PM_DriveMove ( car_t * car , float time , qboolean includeBodies )
{
int i ;
//int t, t1, t2, t3, t4, t5;
if ( car - > initializeOnNextMove )
{
PM_InitializeVehicle ( car , pm - > ps - > origin , pm - > ps - > viewangles , pm - > ps - > velocity ) ;
car - > initializeOnNextMove = qfalse ;
}
//t = trap_Milliseconds();
numTraces = 0 ;
if ( pml . physicsSplit > 2 ) {
Com_Printf ( " forcing end of frame \n " ) ;
return ;
}
if ( time = = 0 )
return ;
// if ( VectorNAN(car->sBody.r) || VectorNAN(car->sBody.v) || VectorNAN(car->sBody.L) )
if ( VectorNAN ( car - > sBody . r ) | | VectorNAN ( car - > sBody . v ) | |
car - > sBody . L [ 0 ] > 1 < < 25 | |
car - > sBody . L [ 1 ] > 1 < < 25 | |
car - > sBody . L [ 2 ] > 1 < < 25 )
{
// Com_Printf( "Blowing up car because of car body\n" );
pm - > damage . damage = 32000 ;
pm - > damage . dflags = DAMAGE_NO_PROTECTION ;
pm - > damage . otherEnt = - 1 ;
pm - > damage . mod = MOD_HIGH_FORCES ;
return ;
}
CP_CURRENT_GRAVITY = ( pm - > ps - > gravity / 800.0f ) * CP_GRAVITY ;
// set spring strengths for "jump" and "crouch"
// FIXME: change this so it works in cgame and game
# if GAME
car - > springStrength = pm - > car_spring * ( CP_FRAME_MASS / 350.0f ) * CP_GRAVITY ;
# else
car - > springStrength = CP_SPRING_STRENGTH ;
# endif
// car->shockStrength = CP_SHOCK_STRENGTH;
if ( pm - > cmd . upmove > 0 ) {
car - > springStrength * = 5.0f ;
}
else if ( pm - > cmd . upmove < 0 ) {
car - > springStrength / = 5.0f ;
}
pm - > damage . damage = 0 ;
pm - > damage . otherEnt = - 1 ;
//t1 = trap_Milliseconds();
// calculate target positions etc
// -------------------------------------------------------------------------
// calculate forces
PM_CalculateForces ( car , & car - > sBody , car - > sPoints , time ) ;
//t2 = trap_Milliseconds();
// apply frame forces to the body
// for (i = FIRST_FRAME_POINT; i < LAST_FRAME_POINT; i++){
for ( i = FIRST_FRAME_POINT ; i < NUM_CAR_POINTS ; i + + ) {
PM_ApplyForce ( & car - > sBody , car - > sPoints [ i ] . netForce , car - > sPoints [ i ] . r ) ;
// }
// for (i = FIRST_FRAME_POINT; i < NUM_CAR_POINTS; i++){
if ( ! car - > sPoints [ i ] . onGround ) continue ;
PM_CarBodyFrictionForces ( car , & car - > sBody , car - > sPoints , i ) ;
if ( VectorNAN ( car - > sPoints [ i ] . netForce ) )
Com_Printf ( " Blowing up car because of car point force on frame \n " ) ;
}
//t3 = trap_Milliseconds();
// print out the forces
/*
if ( pm - > pDebug > 0 & & pm - > pDebug < = 8 ) {
if ( pm - > client )
Com_Printf ( " client sBody ----------------------------------------- \n " ) ;
else
Com_Printf ( " server sBody ----------------------------------------- \n " ) ;
PM_DebugForces ( & car - > sBody , car - > sPoints ) ;
PM_DebugDynamics ( & car - > sBody , car - > sPoints ) ;
}
*/
PM_CalculateTargetBody ( car , & car - > sBody , & car - > tBody , car - > sPoints , car - > tPoints , time ) ;
//t4 = trap_Milliseconds();
PM_Trace_Points ( car , car - > sPoints , car - > tPoints , time ) ;
//t5 = trap_Milliseconds();
// print out the forces
/*
if ( pm - > pDebug > 0 & & pm - > pDebug < = 8 ) {
if ( pm - > client )
Com_Printf ( " client tBody ----------------------------------------- \n " ) ;
else
Com_Printf ( " server tBody ----------------------------------------- \n " ) ;
// PM_DebugForces(&car->tBody, car->tPoints);
PM_DebugDynamics ( & car - > tBody , car - > tPoints ) ;
}
*/
PM_CopyTargetToSource ( & car - > tBody , & car - > sBody , car - > tPoints , car - > sPoints ) ;
// clear forces at the end of the frame instead of at the beginning so we
// can apply forces from nearby explosions between frames
PM_ClearCarForces ( & car - > sBody , car - > sPoints ) ;
// #ifdef CGAME
// Com_Printf( "numTraces %i\n", numTraces );
// #endif
// Com_Printf( "t1 %d, t2 %d, t3 %d, t4 %d, t5 %d\n", t1 - t, t2 - t, t3 - t, t4 - t, t5 - t );
}
