q3rally-team/q3rally
0
0
Fork 0
mirror of https://github.com/Q3Rally-Team/q3rally.git synced 2024-11-26 22:01:50 +00:00
Code Issues Projects Releases Packages Wiki Activity
q3rally/engine/code/game/bg_physics.c
q3rally 62c09831da Changed years in file headers
2021-03-24 20:13:01 +00:00

2388 lines
74 KiB
C
Raw Blame History

/*
===========================================================================
Copyright (C) 1999-2005 Id Software, Inc.
Copyright (C) 2002-2021 Q3Rally Team (Per Thormann - q3rally@gmail.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;
//float impulseDamage;
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);
/*impulseDamage = */PM_ApplyCollision(body, points, hitOrigin, car->sBody.up, body->elasticity);
// 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);
/*impulseDamage = */PM_ApplyCollision( body, points, hitOrigin, normal, 0.0f );
// 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
{
// inside another car
// 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
on the car, integrates to get the next state and traces to handle collisions with
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 );
}
Reference in a new issue View git blame Copy permalink