diff options
Diffstat (limited to 'vphysics')
52 files changed, 23182 insertions, 0 deletions
diff --git a/vphysics/cbase.h b/vphysics/cbase.h new file mode 100644 index 0000000..163ba1d --- /dev/null +++ b/vphysics/cbase.h @@ -0,0 +1,35 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +// system +#include <stdio.h> +#ifdef _XBOX +#include <ctype.h> +#endif + +// Valve +#include "tier0/dbg.h" +#include "mathlib/mathlib.h" +#include "mathlib/vector.h" +#include "utlvector.h" +#include "convert.h" +#include "commonmacros.h" + +// vphysics +#include "vphysics_interface.h" +#include "vphysics_saverestore.h" +#include "vphysics_internal.h" +#include "physics_material.h" +#include "physics_environment.h" +#include "physics_object.h" + +// ivp +#include "ivp_physics.hxx" +#include "ivp_core.hxx" +#include "ivp_templates.hxx" + +// havok
\ No newline at end of file diff --git a/vphysics/convert.cpp b/vphysics/convert.cpp new file mode 100644 index 0000000..5204104 --- /dev/null +++ b/vphysics/convert.cpp @@ -0,0 +1,365 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include <stdio.h> +#include "convert.h" +#include "ivp_cache_object.hxx" +#include "coordsize.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +#if 1 +// game is in inches +vphysics_units_t g_PhysicsUnits = +{ + METERS_PER_INCH, //float unitScaleMeters; // factor that converts game units to meters + 1.0f / METERS_PER_INCH, //float unitScaleMetersInv; // factor that converts meters to game units + 0.25f, // float globalCollisionTolerance; // global collision tolerance in game units + DIST_EPSILON, // float collisionSweepEpsilon; // collision sweep tests clip at this, must be the same as engine's DIST_EPSILON + 1.0f/256.0f, // float collisionSweepIncrementalEpsilon; // near-zero test for incremental steps in collision sweep tests +}; +#else +// game is in meters +vphysics_units_t g_PhysicsUnits = +{ + 1.0f, //float unitScaleMeters; // factor that converts game units to meters + 1.0f, //float unitScaleMetersInv; // factor that converts meters to game units + 0.01f, // float globalCollisionTolerance; // global collision tolerance in game units + 0.01f, // float collisionSweepEpsilon; // collision sweep tests clip at this, must be the same as engine's DIST_EPSILON + 1e-4f, // float collisionSweepIncrementalEpsilon; // near-zero test for incremental steps in collision sweep tests +}; +#endif + +//----------------------------------------------------------------------------- +// HL to IVP conversions +//----------------------------------------------------------------------------- + +void ConvertBoxToIVP( const Vector &mins, const Vector &maxs, Vector &outmins, Vector &outmaxs ) +{ + float tmpZ; + + tmpZ = mins.y; + outmins.y = -HL2IVP(mins.z); + outmins.z = HL2IVP(tmpZ); + outmins.x = HL2IVP(mins.x); + tmpZ = maxs.y; + outmaxs.y = -HL2IVP(maxs.z); + outmaxs.z = HL2IVP(tmpZ); + outmaxs.x = HL2IVP(maxs.x); + + tmpZ = outmaxs.y; + outmaxs.y = outmins.y; + outmins.y = tmpZ; +} + + +void ConvertMatrixToIVP( const matrix3x4_t& matrix, IVP_U_Matrix &out ) +{ + Vector forward, left, up; + + forward.x = matrix[0][0]; + forward.y = matrix[1][0]; + forward.z = matrix[2][0]; + + left.x = matrix[0][1]; + left.y = matrix[1][1]; + left.z = matrix[2][1]; + + up.x = matrix[0][2]; + up.y = matrix[1][2]; + up.z = matrix[2][2]; + + up = -up; + + IVP_U_Float_Point ivpForward, ivpLeft, ivpUp; + + ConvertDirectionToIVP( forward, ivpForward ); + ConvertDirectionToIVP( left, ivpLeft ); + ConvertDirectionToIVP( up, ivpUp ); + + out.set_col( IVP_INDEX_X, &ivpForward ); + out.set_col( IVP_INDEX_Z, &ivpLeft ); + out.set_col( IVP_INDEX_Y, &ivpUp ); + + out.vv.k[0] = HL2IVP(matrix[0][3]); + out.vv.k[1] = -HL2IVP(matrix[2][3]); + out.vv.k[2] = HL2IVP(matrix[1][3]); +} + + +void ConvertRotationToIVP( const QAngle& angles, IVP_U_Matrix3 &out ) +{ + Vector forward, right, up; + IVP_U_Float_Point ivpForward, ivpLeft, ivpUp; + + AngleVectors( angles, &forward, &right, &up ); + // now this is left + right = -right; + + up = -up; + + ConvertDirectionToIVP( forward, ivpForward ); + ConvertDirectionToIVP( right, ivpLeft ); + ConvertDirectionToIVP( up, ivpUp ); + + out.set_col( IVP_INDEX_X, &ivpForward ); + out.set_col( IVP_INDEX_Z, &ivpLeft ); + out.set_col( IVP_INDEX_Y, &ivpUp ); +} + +void ConvertRotationToIVP( const QAngle& angles, IVP_U_Quat &out ) +{ + IVP_U_Matrix3 tmp; + ConvertRotationToIVP( angles, tmp ); + out.set_quaternion( &tmp ); +} + +//----------------------------------------------------------------------------- +// IVP to HL conversions +//----------------------------------------------------------------------------- + +void ConvertMatrixToHL( const IVP_U_Matrix &in, matrix3x4_t& output ) +{ +#if 1 + // copy the row vectors over, swapping z & -y. Also, negate output z + output[0][0] = in.get_elem(0, 0); + output[0][2] = -in.get_elem(0, 1); + output[0][1] = in.get_elem(0, 2); + + output[1][0] = in.get_elem(2, 0); + output[1][2] = -in.get_elem(2, 1); + output[1][1] = in.get_elem(2, 2); + + output[2][0] = -in.get_elem(1, 0); + output[2][2] = in.get_elem(1, 1); + output[2][1] = -in.get_elem(1, 2); + +#else + + // this code is conceptually simpler, but the above is smaller/faster + Vector forward, left, up; + IVP_U_Float_Point out; + + in.get_col( IVP_INDEX_X, &out ); + ConvertDirectionToHL( out, forward ); + in.get_col( IVP_INDEX_Z, &out ); + ConvertDirectionToHL( out, left); + in.get_col( IVP_INDEX_Y, &out ); + ConvertDirectionToHL( out, up ); + up = -up; + + output[0][0] = forward.x; + output[1][0] = forward.y; + output[2][0] = forward.z; + + output[0][1] = left.x; + output[1][1] = left.y; + output[2][1] = left.z; + + output[0][2] = up.x; + output[1][2] = up.y; + output[2][2] = up.z; +#endif + output[0][3] = IVP2HL(in.vv.k[0]); + output[1][3] = IVP2HL(in.vv.k[2]); + output[2][3] = -IVP2HL(in.vv.k[1]); +} + + +void ConvertRotationToHL( const IVP_U_Matrix3 &in, QAngle& angles ) +{ + IVP_U_Float_Point out; + Vector forward, right, up; + + in.get_col( IVP_INDEX_X, &out ); + ConvertDirectionToHL( out, forward ); + in.get_col( IVP_INDEX_Z, &out ); + ConvertDirectionToHL( out, right ); + in.get_col( IVP_INDEX_Y, &out ); + ConvertDirectionToHL( out, up ); + + float xyDist = sqrt( forward[0] * forward[0] + forward[1] * forward[1] ); + + // enough here to get angles? + if ( xyDist > 0.001 ) + { + // (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis + angles[1] = RAD2DEG( atan2( forward[1], forward[0] ) ); + + // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); + angles[0] = RAD2DEG( atan2( -forward[2], xyDist ) ); + + // (roll) z = ATAN( -right.z, up.z ); + angles[2] = RAD2DEG( atan2( -right[2], up[2] ) ) + 180; + } + else // forward is mostly Z, gimbal lock + { + // (yaw) y = ATAN( -right.x, right.y ); -- forward is mostly z, so use right for yaw + angles[1] = RAD2DEG( atan2( right[0], -right[1] ) ); + + // (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) ); + angles[0] = RAD2DEG( atan2( -forward[2], xyDist ) ); + + // Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll) + angles[2] = 180; + } +} + + +void ConvertRotationToHL( const IVP_U_Quat &in, QAngle& angles ) +{ + IVP_U_Matrix3 tmp; + in.set_matrix( &tmp ); + ConvertRotationToHL( tmp, angles ); +} + +// utiltiy code +void TransformIVPToLocal( IVP_U_Point &point, IVP_Real_Object *pObject, bool translate ) +{ + IVP_U_Point tmp = point; + TransformIVPToLocal( tmp, point, pObject, translate ); +} + +void TransformLocalToIVP( IVP_U_Point &point, IVP_Real_Object *pObject, bool translate ) +{ + IVP_U_Point tmp = point; + TransformLocalToIVP( tmp, point, pObject, translate ); +} + + +// UNDONE: use IVP_Cache_Object instead? Measure perf differences. +#define USE_CACHE_OBJECT 0 + + +//----------------------------------------------------------------------------- +// Purpose: This is ONLY for use by the routines below. It's not reentrant!!! +// No threads or recursive calls! +//----------------------------------------------------------------------------- +#if USE_CACHE_OBJECT +#else +static const IVP_U_Matrix *GetTmpObjectMatrix( IVP_Real_Object *pObject ) +{ + static IVP_U_Matrix coreShiftMatrix; + const IVP_U_Matrix *pOut = pObject->get_core()->get_m_world_f_core_PSI(); + + if ( !pObject->flags.shift_core_f_object_is_zero ) + { + coreShiftMatrix.set_matrix( pOut ); + coreShiftMatrix.vmult4( pObject->get_shift_core_f_object(), &coreShiftMatrix.vv ); + return &coreShiftMatrix; + } + return pOut; +} +#endif + +void TransformIVPToLocal( const IVP_U_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate ) +{ +#if USE_CACHE_OBJECT + IVP_Cache_Object *cache = pObject->get_cache_object_no_lock(); + + if ( translate ) + { + cache->transform_position_to_object_coords( &pointIn, &pointOut ); + } + else + { + cache->transform_vector_to_object_coords( &pointIn, &pointOut ); + } +#else + const IVP_U_Matrix *pMatrix = GetTmpObjectMatrix( pObject ); + if ( translate ) + { + pMatrix->inline_vimult4( &pointIn, &pointOut ); + } + else + { + pMatrix->inline_vimult3( &pointIn, &pointOut ); + } +#endif +} + + +void TransformLocalToIVP( const IVP_U_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate ) +{ +#if USE_CACHE_OBJECT + IVP_Cache_Object *cache = pObject->get_cache_object_no_lock(); + + if ( translate ) + { + IVP_U_Float_Point floatPointIn; + floatPointIn.set( &pointIn ); + cache->transform_position_to_world_coords( &floatPointIn, &pointOut ); + } + else + { + cache->transform_vector_to_world_coords( &pointIn, &pointOut ); + } +#else + const IVP_U_Matrix *pMatrix = GetTmpObjectMatrix( pObject ); + + if ( translate ) + { + pMatrix->inline_vmult4( &pointIn, &pointOut ); + } + else + { + pMatrix->inline_vmult3( &pointIn, &pointOut ); + } +#endif +} + +void TransformLocalToIVP( const IVP_U_Float_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate ) +{ +#if USE_CACHE_OBJECT + IVP_Cache_Object *cache = pObject->get_cache_object_no_lock(); + + if ( translate ) + { + cache->transform_position_to_world_coords( &pointIn, &pointOut ); + } + else + { + IVP_U_Point doublePointIn; + doublePointIn.set( &pointIn ); + cache->transform_vector_to_world_coords( &doublePointIn, &pointOut ); + } +#else + const IVP_U_Matrix *pMatrix = GetTmpObjectMatrix( pObject ); + IVP_U_Float_Point out; + + if ( translate ) + { + pMatrix->inline_vmult4( &pointIn, &out ); + } + else + { + pMatrix->inline_vmult3( &pointIn, &out ); + } + pointOut.set( &out ); +#endif +} + +void TransformLocalToIVP( const IVP_U_Float_Point &pointIn, IVP_U_Float_Point &pointOut, IVP_Real_Object *pObject, bool translate ) +{ + IVP_U_Point tmpOut; + TransformLocalToIVP( pointIn, tmpOut, pObject, translate ); + pointOut.set( &tmpOut ); +} + +static char axisMap[] = {0,2,1,3}; + +int ConvertCoordinateAxisToIVP( int axisIndex ) +{ + return axisIndex < 4 ? axisMap[axisIndex] : 0; +} + +int ConvertCoordinateAxisToHL( int axisIndex ) +{ + return axisIndex < 4 ? axisMap[axisIndex] : 0; +} + diff --git a/vphysics/convert.h b/vphysics/convert.h new file mode 100644 index 0000000..8ae7561 --- /dev/null +++ b/vphysics/convert.h @@ -0,0 +1,279 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef CONVERT_H +#define CONVERT_H +#pragma once + +#include "mathlib/vector.h" +#include "mathlib/mathlib.h" +#include "ivp_physics.hxx" +struct cplane_t; +#include "vphysics_interface.h" + +// UNDONE: Remove all conversion/scaling +// Convert our units (inches) to IVP units (meters) +struct vphysics_units_t +{ + float unitScaleMeters; // factor that converts game units to meters + float unitScaleMetersInv; // factor that converts meters to game units + float globalCollisionTolerance; // global collision tolerance in game units + float collisionSweepEpsilon; // collision sweep tests clip at this, must be the same as engine's DIST_EPSILON + float collisionSweepIncrementalEpsilon; // near-zero test for incremental steps in collision sweep tests +}; + +extern vphysics_units_t g_PhysicsUnits; + +#define HL2IVP_FACTOR g_PhysicsUnits.unitScaleMeters +#define IVP2HL(x) (float)(x * (g_PhysicsUnits.unitScaleMetersInv)) +#define HL2IVP(x) (double)(x * HL2IVP_FACTOR) + +// Convert HL engine units to IVP units +inline void ConvertPositionToIVP( const Vector &in, IVP_U_Float_Point &out ) +{ + float tmpZ; + + tmpZ = in[1]; + + out.k[0] = HL2IVP(in[0]); + out.k[1] = -HL2IVP(in[2]); + out.k[2] = HL2IVP(tmpZ); +} + +inline void ConvertPositionToIVP( const Vector &in, IVP_U_Point &out ) +{ + float tmpZ; + + tmpZ = in[1]; + + out.k[0] = HL2IVP(in[0]); + out.k[1] = -HL2IVP(in[2]); + out.k[2] = HL2IVP(tmpZ); +} + +inline void ConvertPositionToIVP( const Vector &in, IVP_U_Float_Point3 &out ) +{ + float tmpZ; + + tmpZ = in[1]; + + out.k[0] = HL2IVP(in[0]); + out.k[1] = -HL2IVP(in[2]); + out.k[2] = HL2IVP(tmpZ); +} + +inline void ConvertPositionToIVP( float &x, float &y, float &z ) +{ + float tmpZ; + + tmpZ = y; + y = -HL2IVP(z); + z = HL2IVP(tmpZ); + x = HL2IVP(x); +} + +inline void ConvertDirectionToIVP( const Vector &in, IVP_U_Float_Point &out ) +{ + float tmpZ; + + tmpZ = in[1]; + + out.k[0] = in[0]; + out.k[1] = -in[2]; + out.k[2] = tmpZ; +} + + +inline void ConvertDirectionToIVP( const Vector &in, IVP_U_Point &out ) +{ + float tmpZ; + + tmpZ = in[1]; + + out.k[0] = in[0]; + out.k[1] = -in[2]; + out.k[2] = tmpZ; +} + + +// forces are handled the same as positions & velocities (scaled by distance conversion factor) +#define ConvertForceImpulseToIVP ConvertPositionToIVP +#define ConvertForceImpulseToHL ConvertPositionToHL + +inline float ConvertAngleToIVP( float angleIn ) +{ + return DEG2RAD(angleIn); +} + +inline void ConvertAngularImpulseToIVP( const AngularImpulse &in, IVP_U_Float_Point &out ) +{ + float tmpZ; + + tmpZ = in[1]; + + out.k[0] = DEG2RAD(in[0]); + out.k[1] = -DEG2RAD(in[2]); + out.k[2] = DEG2RAD(tmpZ); +} + + +inline float ConvertDistanceToIVP( float distance ) +{ + return HL2IVP( distance ); +} + +inline void ConvertPlaneToIVP( const Vector &pNormal, float dist, IVP_U_Hesse &plane ) +{ + ConvertDirectionToIVP( pNormal, (IVP_U_Point &)plane ); + // HL stores planes as Ax + By + Cz = D + // IVP stores them as Ax + BY + Cz + D = 0 + plane.hesse_val = -ConvertDistanceToIVP( dist ); +} + + +inline void ConvertPlaneToIVP( const Vector &pNormal, float dist, IVP_U_Float_Hesse &plane ) +{ + ConvertDirectionToIVP( pNormal, (IVP_U_Float_Point &)plane ); + // HL stores planes as Ax + By + Cz = D + // IVP stores them as Ax + BY + Cz + D = 0 + plane.hesse_val = -ConvertDistanceToIVP( dist ); +} + +inline float ConvertDensityToIVP( float density ) +{ + return density; +} + +// in convert.cpp +extern void ConvertMatrixToIVP( const matrix3x4_t& matrix, IVP_U_Matrix &out ); +extern void ConvertRotationToIVP( const QAngle &angles, IVP_U_Matrix3 &out ); +extern void ConvertRotationToIVP( const QAngle& angles, IVP_U_Quat &out ); +extern void ConvertBoxToIVP( const Vector &mins, const Vector &maxs, Vector &outmins, Vector &outmaxs ); +extern int ConvertCoordinateAxisToIVP( int axisIndex ); +extern int ConvertCoordinateAxisToHL( int axisIndex ); + +// IVP to HL conversions +inline void ConvertPositionToHL( const IVP_U_Point &point, Vector& out ) +{ + float tmpY = IVP2HL(point.k[2]); + out[2] = -IVP2HL(point.k[1]); + out[1] = tmpY; + out[0] = IVP2HL(point.k[0]); +} + +inline void ConvertPositionToHL( const IVP_U_Float_Point &point, Vector& out ) +{ + float tmpY = IVP2HL(point.k[2]); + out[2] = -IVP2HL(point.k[1]); + out[1] = tmpY; + out[0] = IVP2HL(point.k[0]); +} + +inline void ConvertPositionToHL( const IVP_U_Float_Point3 &point, Vector& out ) +{ + float tmpY = IVP2HL(point.k[2]); + out[2] = -IVP2HL(point.k[1]); + out[1] = tmpY; + out[0] = IVP2HL(point.k[0]); +} + +inline void ConvertDirectionToHL( const IVP_U_Point &point, Vector& out ) +{ + float tmpY = point.k[2]; + out[2] = -point.k[1]; + out[1] = tmpY; + out[0] = point.k[0]; +} + + +inline void ConvertDirectionToHL( const IVP_U_Float_Point &point, Vector& out ) +{ + float tmpY = point.k[2]; + out[2] = -point.k[1]; + out[1] = tmpY; + out[0] = point.k[0]; +} + + +inline float ConvertAngleToHL( float angleIn ) +{ + return RAD2DEG(angleIn); +} + +inline void ConvertAngularImpulseToHL( const IVP_U_Float_Point &point, AngularImpulse &out ) +{ + float tmpY = point.k[2]; + out[2] = -RAD2DEG(point.k[1]); + out[1] = RAD2DEG(tmpY); + out[0] = RAD2DEG(point.k[0]); +} + +inline float ConvertDistanceToHL( float distance ) +{ + return IVP2HL( distance ); +} + + +// NOTE: Converts in place +inline void ConvertPlaneToHL( cplane_t &plane ) +{ + IVP_U_Float_Hesse tmp(plane.normal.x, plane.normal.y, plane.normal.z, -plane.dist); + ConvertDirectionToHL( (IVP_U_Float_Point &)tmp, plane.normal ); + // HL stores planes as Ax + By + Cz = D + // IVP stores them as Ax + BY + Cz + D = 0 + plane.dist = -ConvertDistanceToHL( tmp.hesse_val ); +} + +inline void ConvertPlaneToHL( const IVP_U_Float_Hesse &plane, Vector *pNormalOut, float *pDistOut ) +{ + if ( pNormalOut ) + { + ConvertDirectionToHL( plane, *pNormalOut ); + } + // HL stores planes as Ax + By + Cz = D + // IVP stores them as Ax + BY + Cz + D = 0 + if ( pDistOut ) + { + *pDistOut = -ConvertDistanceToHL( plane.hesse_val ); + } +} + +inline float ConvertVolumeToHL( float volume ) +{ + float factor = IVP2HL(1.0); + factor = (factor * factor * factor); + return factor * volume; +} + +#define INSQR_PER_METERSQR (1.f / (METERS_PER_INCH*METERS_PER_INCH)) +inline float ConvertEnergyToHL( float energy ) +{ + return energy * INSQR_PER_METERSQR; +} + +inline void IVP_Float_PointAbs( IVP_U_Float_Point &out, const IVP_U_Float_Point &in ) +{ + out.k[0] = fabsf( in.k[0] ); + out.k[1] = fabsf( in.k[1] ); + out.k[2] = fabsf( in.k[2] ); +} + +// convert.cpp +extern void ConvertRotationToHL( const IVP_U_Matrix3 &in, QAngle &angles ); +extern void ConvertMatrixToHL( const IVP_U_Matrix &in, matrix3x4_t& output ); +extern void ConvertRotationToHL( const IVP_U_Quat &in, QAngle& angles ); + +extern void TransformIVPToLocal( IVP_U_Point &pointInOut, IVP_Real_Object *pObject, bool translate ); +extern void TransformLocalToIVP( IVP_U_Point &pointInOut, IVP_Real_Object *pObject, bool translate ); + +extern void TransformIVPToLocal( const IVP_U_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate ); +extern void TransformLocalToIVP( const IVP_U_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate ); + +extern void TransformLocalToIVP( const IVP_U_Float_Point &pointIn, IVP_U_Point &pointOut, IVP_Real_Object *pObject, bool translate ); +extern void TransformLocalToIVP( const IVP_U_Float_Point &pointIn, IVP_U_Float_Point &pointOut, IVP_Real_Object *pObject, bool translate ); + +#endif // CONVERT_H diff --git a/vphysics/ledgewriter.cpp b/vphysics/ledgewriter.cpp new file mode 100644 index 0000000..2e313b8 --- /dev/null +++ b/vphysics/ledgewriter.cpp @@ -0,0 +1,517 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: low-level code to write IVP_Compact_Ledge/IVP_Compact_Triangle. +// also includes code to pack/unpack outer hull ledges to 8-bit rep +// +//============================================================================= +#include "cbase.h" +#include "convert.h" + +#include <ivp_surface_manager.hxx> +#include <ivp_surman_polygon.hxx> +#include <ivp_template_surbuild.hxx> +#include <ivp_compact_surface.hxx> +#include <ivp_compact_ledge.hxx> + +#include "utlbuffer.h" +#include "ledgewriter.h" + +// gets the max vertex index referenced by a compact ledge +static int MaxLedgeVertIndex( const IVP_Compact_Ledge *pLedge ) +{ + int maxIndex = -1; + for ( int i = 0; i < pLedge->get_n_triangles(); i++ ) + { + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i; + for ( int j = 0; j < 3; j++ ) + { + int ivpIndex = pTri->get_edge(j)->get_start_point_index(); + maxIndex = max(maxIndex, ivpIndex); + } + } + return maxIndex; +} + + +struct vertmap_t +{ + + CUtlVector<int> map; + int minRef; + int maxRef; +}; + +// searches pVerts for each vert used by pLedge and builds a one way map from ledge indices to pVerts indices +// NOTE: pVerts is in HL coords, pLedge is in IVP coords +static void BuildVertMap( vertmap_t &out, const Vector *pVerts, int vertexCount, const IVP_Compact_Ledge *pLedge ) +{ + out.map.EnsureCount(MaxLedgeVertIndex(pLedge)+1); + for ( int i = 0; i < out.map.Count(); i++ ) + { + out.map[i] = -1; + } + out.minRef = vertexCount; + out.maxRef = 0; + const IVP_Compact_Poly_Point *pVertList = pLedge->get_point_array(); + for ( int i = 0; i < pLedge->get_n_triangles(); i++ ) + { + // iterate each triangle, for each referenced vert that hasn't yet been mapped, search for the nearest match + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i; + for ( int j = 0; j < 3; j++ ) + { + int ivpIndex = pTri->get_edge(j)->get_start_point_index(); + if ( out.map[ivpIndex] < 0 ) + { + int index = -1; + Vector tmp; + ConvertPositionToHL( &pVertList[ivpIndex], tmp); + float minDist = 1e24; + for ( int k = 0; k < vertexCount; k++ ) + { + float dist = (tmp-pVerts[k]).Length(); + if ( dist < minDist ) + { + index = k; + minDist = dist; + } + } + Assert(minDist<0.1f); + out.map[ivpIndex] = index; + out.minRef = min(out.minRef, index); + out.maxRef = max(out.maxRef, index); + } + } + } +} + + +// Each IVP_Compact_Triangle and IVP_Compact_Edge occupies an index +// 0,1,2,3 is tri, edge, edge, edge (tris and edges are both 16 bytes) +// So you can just add the index to get_first_triangle to get a pointer +inline int EdgeIndex( const IVP_Compact_Ledge *pLedge, const IVP_Compact_Edge *pEdge ) +{ + return pEdge - (const IVP_Compact_Edge *)pLedge->get_first_triangle(); +} + +// Builds a packedhull_t from a IVP_Compact_Ledge. Assumes that the utlbuffer points at the memory following pHull (pHull is the header, utlbuffer is the body) +void PackLedgeIntoBuffer( packedhull_t *pHull, CUtlBuffer &buf, const IVP_Compact_Ledge *pLedge, const virtualmeshlist_t &list ) +{ + if ( !pLedge ) + return; + + // The lists store the ivp index of each element to be written out + // The maps store the output packed index for each ivp index + CUtlVector<int> triangleList, triangleMap; + CUtlVector<int> edgeList, edgeMap; + vertmap_t vertMap; + BuildVertMap( vertMap, list.pVerts, list.vertexCount, pLedge ); + pHull->baseVert = vertMap.minRef; + // clear the maps + triangleMap.EnsureCount(pLedge->get_n_triangles()); + for ( int i = 0; i < triangleMap.Count(); i++ ) + { + triangleMap[i] = -1; + } + edgeMap.EnsureCount(pLedge->get_n_triangles()*4); // each triangle also occupies an edge index + for ( int i = 0; i < edgeMap.Count(); i++ ) + { + edgeMap[i] = -1; + } + + // we're going to reorder the triangles and edges so that the ones marked virtual + // appear first in the list. This way we only need a virtual count, not a per-item + // flag. + + // also, the edges are stored relative to the first triangle that references them + // so an edge from 0->1 means that the first triangle that references the edge is 0->1 and the + // second triangle is 1->0. This way we store half the edges and the winged edge pointers are implicit + + // sort triangles in two passes + for ( int i = 0; i < pLedge->get_n_triangles(); i++ ) + { + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i; + if ( pTri->get_is_virtual() ) + { + triangleMap[i] = triangleList.AddToTail(i); + } + } + pHull->vtriCount = triangleList.Count(); + for ( int i = 0; i < pLedge->get_n_triangles(); i++ ) + { + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i; + if ( !pTri->get_is_virtual() ) + { + triangleMap[i] = triangleList.AddToTail(i); + } + } + // sort edges in two passes + for ( int i = 0; i < pLedge->get_n_triangles(); i++ ) + { + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + triangleList[i]; + for ( int j = 0; j < 3; j++ ) + { + const IVP_Compact_Edge *pEdge = pTri->get_edge(j); + if ( pEdge->get_is_virtual() && edgeMap[EdgeIndex(pLedge, pEdge->get_opposite())] < 0 ) + { + edgeMap[EdgeIndex(pLedge, pEdge)] = edgeList.AddToTail(EdgeIndex(pLedge, pEdge)); + } + } + } + pHull->vedgeCount = edgeList.Count(); + + for ( int i = 0; i < pLedge->get_n_triangles(); i++ ) + { + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + triangleList[i]; + for ( int j = 0; j < 3; j++ ) + { + const IVP_Compact_Edge *pEdge = pTri->get_edge(j); + int index = EdgeIndex(pLedge, pEdge); + int oppositeIndex = EdgeIndex(pLedge, pEdge->get_opposite()); + if ( !pEdge->get_is_virtual() && edgeMap[oppositeIndex] < 0 ) + { + edgeMap[index] = edgeList.AddToTail(index); + } + if ( edgeMap[index] < 0 ) + { + Assert(edgeMap[oppositeIndex] >= 0); + edgeMap[index] = edgeMap[oppositeIndex]; + } + } + } + Assert( edgeList.Count() == pHull->edgeCount ); + + // now write the packed triangles + for ( int i = 0; i < pHull->triangleCount; i++ ) + { + packedtriangle_t tri; + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + triangleList[i]; + const IVP_Compact_Edge *pEdge; + pEdge = pTri->get_edge(0); + tri.opposite = triangleMap[pTri->get_pierce_index()]; + Assert(tri.opposite<pHull->triangleCount); + tri.e0 = edgeMap[EdgeIndex(pLedge, pEdge)]; + pEdge = pTri->get_edge(1); + tri.e1 = edgeMap[EdgeIndex(pLedge, pEdge)]; + pEdge = pTri->get_edge(2); + tri.e2 = edgeMap[EdgeIndex(pLedge, pEdge)]; + Assert(tri.e0<pHull->edgeCount); + Assert(tri.e1<pHull->edgeCount); + Assert(tri.e2<pHull->edgeCount); + buf.Put(&tri, sizeof(tri)); + } + // now write the packed edges + for ( int i = 0; i < pHull->edgeCount; i++ ) + { + packededge_t edge; + const IVP_Compact_Edge *pEdge = (const IVP_Compact_Edge *)pLedge->get_first_triangle() + edgeList[i]; + Assert((edgeList[i]&3) != 0); // must not be a triangle + + int v0 = vertMap.map[pEdge->get_start_point_index()] - pHull->baseVert; + int v1 = vertMap.map[pEdge->get_next()->get_start_point_index()] - pHull->baseVert; + Assert(v0>=0 && v0<256); + Assert(v1>=0 && v1<256); + edge.v0 = v0; + edge.v1 = v1; + buf.Put(&edge, sizeof(edge)); + } +} + + +// decompress packed hull into a compact ledge +void CVPhysicsVirtualMeshWriter::UnpackCompactLedgeFromHull( IVP_Compact_Ledge *pLedge, int materialIndex, const IVP_Compact_Poly_Point *pPointList, const virtualmeshhull_t *pHullHeader, int hullIndex, bool isVirtualLedge ) +{ + const packedhull_t *pHull = pHullHeader->GetPackedHull(hullIndex); + const packedtriangle_t *pPackedTris = pHullHeader->GetPackedTriangles(hullIndex); + // write the ledge + pLedge->set_offset_ledge_points( (int)((char *)pPointList - (char *)pLedge) ); // byte offset from 'this' to (ledge) point array + pLedge->set_is_compact( IVP_TRUE ); + pLedge->set_size(sizeof(IVP_Compact_Ledge) + sizeof(IVP_Compact_Triangle)*pHull->triangleCount); // <0 indicates a non compact compact ledge + pLedge->n_triangles = pHull->triangleCount; + pLedge->has_chilren_flag = isVirtualLedge ? IVP_TRUE : IVP_FALSE; + + // Make the offset -pLedge so the result is a NULL ledgetree node - we haven't needed to create one of these as of yet + pLedge->ledgetree_node_offset = -((int)pLedge); + + // keep track of which triangle edge referenced this edge (so the next one can swap the order and point to the first one) + int forwardEdgeIndex[255]; + for ( int i = 0; i < pHull->edgeCount; i++ ) + { + forwardEdgeIndex[i] = -1; + } + packededge_t *pPackedEdges = (packededge_t *)(pPackedTris + pHull->triangleCount); + IVP_Compact_Triangle *pOut = pLedge->get_first_triangle(); + // now write the compact triangles and their edges + int baseVert = pHull->baseVert; + for ( int i = 0; i < pHull->triangleCount; i++ ) + { + pOut[i].set_tri_index(i); + pOut[i].set_material_index(materialIndex); + pOut[i].set_is_virtual( i < pHull->vtriCount ? IVP_TRUE : IVP_FALSE ); + pOut[i].set_pierce_index(pPackedTris[i].opposite); + Assert(pPackedTris[i].opposite<pHull->triangleCount); + int edges[3] = {pPackedTris[i].e0, pPackedTris[i].e1, pPackedTris[i].e2}; + for ( int j = 0; j < 3; j++ ) + { + Assert(edges[j]<pHull->edgeCount); + if ( forwardEdgeIndex[edges[j]] < 0 ) + { + // this is the first triangle to use this edge, so it's forward (and the other triangle sharing (opposite edge pointer) is unknown) + int startVert = pPackedEdges[edges[j]].v0 + baseVert; + pOut[i].c_three_edges[j].set_start_point_index(startVert); + pOut[i].c_three_edges[j].set_is_virtual( edges[j] < pHull->vedgeCount ? IVP_TRUE : IVP_FALSE ); + forwardEdgeIndex[edges[j]] = EdgeIndex(pLedge, &pOut[i].c_three_edges[j]); + } + else + { + // this is the second triangle to use this edge, so it's reversed (and the other triangle sharing is in the forward edge table) + int oppositeIndex = forwardEdgeIndex[edges[j]]; + + int startVert = pPackedEdges[edges[j]].v1 + baseVert; + pOut[i].c_three_edges[j].set_start_point_index(startVert); + pOut[i].c_three_edges[j].set_is_virtual( edges[j] < pHull->vedgeCount ? IVP_TRUE : IVP_FALSE ); + // now build the links between the triangles sharing this edge + int thisEdgeIndex = EdgeIndex( pLedge, &pOut[i].c_three_edges[j] ); + pOut[i].c_three_edges[j].set_opposite_index( oppositeIndex - thisEdgeIndex ); + pOut[i].c_three_edges[j].get_opposite()->set_opposite_index( thisEdgeIndex - oppositeIndex ); + } + } + } +} + +// low-level code to initialize a 2-sided triangle +static void InitTriangle( IVP_Compact_Triangle *pTri, int index, int materialIndex, int v0, int v1, int v2, int opp0, int opp1, int opp2 ) +{ + pTri->set_tri_index(index); + pTri->set_material_index(materialIndex); + + pTri->c_three_edges[0].set_start_point_index(v0); + pTri->c_three_edges[1].set_start_point_index(v1); + pTri->c_three_edges[2].set_start_point_index(v2); + + pTri->c_three_edges[0].set_opposite_index(opp0); + pTri->c_three_edges[1].set_opposite_index(opp1); + pTri->c_three_edges[2].set_opposite_index(opp2); +} + +void CVPhysicsVirtualMeshWriter::InitTwoSidedTriangleLege( triangleledge_t *pOut, const IVP_Compact_Poly_Point *pPoints, int v0, int v1, int v2, int materialIndex ) +{ + IVP_Compact_Ledge *pLedge = &pOut->ledge; + pLedge->set_offset_ledge_points( (int)((char *)pPoints - (char *)pLedge) ); // byte offset from 'this' to (ledge) point array + pLedge->set_is_compact( IVP_TRUE ); + pLedge->set_size(sizeof(triangleledge_t)); // <0 indicates a non compact compact ledge + pLedge->n_triangles = 2; + pLedge->has_chilren_flag = IVP_FALSE; + // triangles + InitTriangle( &pOut->faces[0], 0, materialIndex, v0, v1, v2, 6, 4, 2 ); + InitTriangle( &pOut->faces[1], 1, materialIndex, v0, v2, v1, -2, -4, -6); + pOut->faces[0].set_pierce_index(1); + pOut->faces[1].set_pierce_index(0); +} + +bool CVPhysicsVirtualMeshWriter::LedgeCanBePacked(const IVP_Compact_Ledge *pLedge, const virtualmeshlist_t &list) +{ + int edgeCount = pLedge->get_n_triangles() * 3; + if ( edgeCount > 512 ) + return false; + vertmap_t vertMap; + BuildVertMap( vertMap, list.pVerts, list.vertexCount, pLedge ); + if ( (vertMap.maxRef - vertMap.minRef) > 255 ) + return false; + return true; +} + +// this builds a packed hull array from a compact ledge array (needs the virtualmeshlist for reference) +virtualmeshhull_t *CVPhysicsVirtualMeshWriter::CreatePackedHullFromLedges( const virtualmeshlist_t &list, const IVP_Compact_Ledge **pLedges, int ledgeCount ) +{ + int triCount = 0; + int edgeCount = 0; + for ( int i = 0; i < ledgeCount; i++ ) + { + triCount += pLedges[i]->get_n_triangles(); + edgeCount += (pLedges[i]->get_n_triangles() * 3)/2; + Assert(LedgeCanBePacked(pLedges[i], list)); + } + + unsigned int totalSize = sizeof(packedtriangle_t)*triCount + sizeof(packededge_t)*edgeCount + sizeof(packedhull_t)*ledgeCount + sizeof(virtualmeshhull_t); + byte *pBuf = new byte[totalSize]; + + CUtlBuffer buf; + buf.SetExternalBuffer( pBuf, totalSize, 0, 0 ); + + if ( 1 ) + { + virtualmeshhull_t tmp; + Q_memset( &tmp, 0, sizeof(tmp) ); + tmp.hullCount = ledgeCount; + buf.Put(&tmp, sizeof(tmp)); + } + + // write the headers + Assert(ledgeCount < 16); + packedhull_t *pHulls[16]; + for ( int i = 0; i < ledgeCount; i++ ) + { + pHulls[i] = (packedhull_t *)buf.PeekPut(); + packedhull_t hull; + hull.triangleCount = pLedges[i]->get_n_triangles(); + hull.edgeCount = (hull.triangleCount * 3) / 2; + buf.Put(&hull, sizeof(hull)); + } + + // write the data itself + for ( int i = 0; i < ledgeCount; i++ ) + { + PackLedgeIntoBuffer( pHulls[i], buf, pLedges[i], list ); + } + + return (virtualmeshhull_t *)pBuf; +} + +// frees the memory associated with this packed hull +void CVPhysicsVirtualMeshWriter::DestroyPackedHull( virtualmeshhull_t *pHull ) +{ + byte *pData = (byte *)pHull; + delete[] pData; +} + + +unsigned int CVPhysicsVirtualMeshWriter::UnpackLedgeListFromHull( byte *pOut, virtualmeshhull_t *pHull, IVP_Compact_Poly_Point *pPoints ) +{ + unsigned int memOffset = 0; + for ( int i = 0; i < pHull->hullCount; i++ ) + { + IVP_Compact_Ledge *pHullLedge = (IVP_Compact_Ledge *)(pOut + memOffset); + CVPhysicsVirtualMeshWriter::UnpackCompactLedgeFromHull( pHullLedge, 0, pPoints, pHull, i, true ); + memOffset += pHullLedge->get_size(); + } + return memOffset; +} + + +/* + +#define DUMP_FILES 1 +static bool DumpListToGLView( const char *pFilename, const virtualmeshlist_t &list ) +{ +#if DUMP_FILES + FILE *fp = fopen( pFilename, "a+" ); + for ( int i = 0; i < list.triangleCount; i++ ) + { + fprintf( fp, "3\n" ); + fprintf( fp, "%6.3f %6.3f %6.3f 1 0 0\n", list.pVerts[list.indices[i*3+0]].x, list.pVerts[list.indices[i*3+0]].y, list.pVerts[list.indices[i*3+0]].z ); + fprintf( fp, "%6.3f %6.3f %6.3f 0 1 0\n", list.pVerts[list.indices[i*3+1]].x, list.pVerts[list.indices[i*3+1]].y, list.pVerts[list.indices[i*3+1]].z ); + fprintf( fp, "%6.3f %6.3f %6.3f 0 0 1\n", list.pVerts[list.indices[i*3+2]].x, list.pVerts[list.indices[i*3+2]].y, list.pVerts[list.indices[i*3+2]].z ); + } + fclose(fp); +#endif + return true; +} + +static bool DumpLedgeToGLView( const char *pFilename, const IVP_Compact_Ledge *pLedge, float r=1.0f, float g=1.0f, float b=1.0f, float offset=0.0f ) +{ +#if DUMP_FILES + FILE *fp = fopen( pFilename, "a+" ); + int ivpIndex; + Vector tmp[3]; + const IVP_Compact_Poly_Point *pPoints = pLedge->get_point_array(); + for ( int i = 0; i < pLedge->get_n_triangles(); i++ ) + { + // iterate each triangle, for each referenced vert that hasn't yet been mapped, search for the nearest match + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i; + ivpIndex = pTri->get_edge(2)->get_start_point_index(); + ConvertPositionToHL( &pPoints[ivpIndex], tmp[0] ); + ivpIndex = pTri->get_edge(1)->get_start_point_index(); + ConvertPositionToHL( &pPoints[ivpIndex], tmp[1] ); + ivpIndex = pTri->get_edge(0)->get_start_point_index(); + ConvertPositionToHL( &pPoints[ivpIndex], tmp[2] ); + tmp[0].x += offset; + tmp[1].x += offset; + tmp[2].x += offset; + fprintf( fp, "2\n" ); + fprintf( fp, "%6.3f %6.3f %6.3f %.1f %.1f %.1f\n", tmp[0].x, tmp[0].y, tmp[0].z, r, g, b ); + fprintf( fp, "%6.3f %6.3f %6.3f %.1f %.1f %.1f\n", tmp[1].x, tmp[1].y, tmp[1].z, r, g, b ); + fprintf( fp, "2\n" ); + fprintf( fp, "%6.3f %6.3f %6.3f %.1f %.1f %.1f\n", tmp[1].x, tmp[1].y, tmp[1].z, r, g, b ); + fprintf( fp, "%6.3f %6.3f %6.3f %.1f %.1f %.1f\n", tmp[2].x, tmp[2].y, tmp[2].z, r, g, b ); + fprintf( fp, "2\n" ); + fprintf( fp, "%6.3f %6.3f %6.3f %.1f %.1f %.1f\n", tmp[2].x, tmp[2].y, tmp[2].z, r, g, b ); + fprintf( fp, "%6.3f %6.3f %6.3f %.1f %.1f %.1f\n", tmp[0].x, tmp[0].y, tmp[0].z, r, g, b ); + } + fclose( fp ); +#endif + return true; +} + +static int ComputeSize( virtualmeshhull_t *pHeader ) +{ + packedhull_t *pHull = (packedhull_t *)(pHeader+1); + unsigned int size = pHeader->hullCount * sizeof(IVP_Compact_Ledge); + for ( int i = 0; i < pHeader->hullCount; i++ ) + { + size += sizeof(IVP_Compact_Triangle) * pHull[i].triangleCount; + } + return size; +} + +bool CVPhysicsVirtualMeshWriter::CheckHulls( virtualmeshhull_t *pHull0, virtualmeshhull_t *pHull1, const virtualmeshlist_t &list ) +{ + for ( int i = 0; i < pHull0->hullCount; i++ ) + { + const packedhull_t *pP0 = pHull0->GetPackedHull(i); + const packedhull_t *pP1 = pHull1->GetPackedHull(i); + Assert(pP0->triangleCount == pP1->triangleCount); + Assert(pP0->vtriCount == pP1->vtriCount); + Assert(pP0->edgeCount == pP1->edgeCount); + Assert(pP0->vedgeCount == pP1->vedgeCount); + Assert(pP0->baseVert == pP1->baseVert); + const packedtriangle_t *pTri0 = pHull0->GetPackedTriangles( i ); + const packedtriangle_t *pTri1 = pHull1->GetPackedTriangles( i ); + for ( int j = 0; j < pP0->triangleCount; j++ ) + { + Assert(pTri0[j].e0 == pTri1[j].e0); + Assert(pTri0[j].e1 == pTri1[j].e1); + Assert(pTri0[j].e2 == pTri1[j].e2); + Assert(pTri0[j].opposite == pTri1[j].opposite); + } + } + { + int size0 = ComputeSize(pHull0); + int pointSize0 = sizeof(IVP_Compact_Poly_Point) * list.vertexCount; + byte *pMem0 = (byte *)ivp_malloc_aligned( size0+pointSize0, 16 ); + IVP_Compact_Poly_Point *pPoints = (IVP_Compact_Poly_Point *)pMem0; + IVP_Compact_Ledge *pLedge0 = (IVP_Compact_Ledge *)(pPoints + list.vertexCount); + for ( int i = 0; i < list.vertexCount; i++ ) + { + ConvertPositionToIVP( list.pVerts[i], pPoints[i] ); + } + UnpackLedgeListFromHull( (byte *)pLedge0, pHull0, pPoints ); + for ( int i = 0; i < pHull0->hullCount; i++ ) + { + if ( i == i ) DumpLedgeToGLView( "c:\\jay.txt", pLedge0, 1, 0, 0, 0 ); + pLedge0 = (IVP_Compact_Ledge *)( ((byte *)pLedge0 ) + pLedge0->get_size() ); + } + ivp_free_aligned(pMem0); + } + + { + int size1 = ComputeSize(pHull1); + int pointSize1 = sizeof(IVP_Compact_Poly_Point) * list.vertexCount; + byte *pMem1 = (byte *)ivp_malloc_aligned( size1+pointSize1, 16 ); + IVP_Compact_Poly_Point *pPoints = (IVP_Compact_Poly_Point *)pMem1; + IVP_Compact_Ledge *pLedge1 = (IVP_Compact_Ledge *)(pPoints + list.vertexCount); + for ( int i = 0; i < list.vertexCount; i++ ) + { + ConvertPositionToIVP( list.pVerts[i], pPoints[i] ); + } + UnpackLedgeListFromHull( (byte *)pLedge1, pHull1, pPoints ); + for ( int i = 0; i < pHull1->hullCount; i++ ) + { + if ( i == i ) DumpLedgeToGLView( "c:\\jay.txt", pLedge1, 0, 1, 0, 1024 ); + pLedge1 = (IVP_Compact_Ledge *)( ((byte *)pLedge1 ) + pLedge1->get_size() ); + } + ivp_free_aligned(pMem1); + } + return true; +} + +*/ diff --git a/vphysics/ledgewriter.h b/vphysics/ledgewriter.h new file mode 100644 index 0000000..47a5f29 --- /dev/null +++ b/vphysics/ledgewriter.h @@ -0,0 +1,110 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//============================================================================= + +#ifndef LEDGEWRITER_H +#define LEDGEWRITER_H +#ifdef _WIN32 +#pragma once +#endif + +#include "vphysics/virtualmesh.h" + +// 2-sided triangle ledge rep +struct triangleledge_t +{ + IVP_Compact_Ledge ledge; + IVP_Compact_Triangle faces[2]; +}; + +// minimum footprint convex hull rep. Assume 8-bits of index space per edge/triangle/vert +// NOTE: EACH ELEMENT OF THESE STRUCTS MUST BE 8-bits OR YOU HAVE TO WRITE SWAPPING CODE FOR +// THE X360 IMPLEMENTATION. THERE IS NO SUCH CODE AS OF NOW. +#pragma pack(1) +struct packedtriangle_t +{ + byte e0; // only bytes allowed see above + byte e1; + byte e2; + byte opposite; +}; + +struct packededge_t +{ + byte v0; // only bytes allowed see above + byte v1; +}; + +struct packedhull_t +{ + byte triangleCount; // only bytes allowed see above + byte vtriCount; + byte edgeCount; + byte vedgeCount; + byte baseVert; + inline size_t DataSize() const + { + return (sizeof(packedtriangle_t) * triangleCount) + (sizeof(packededge_t)*edgeCount); + } +}; + +struct virtualmeshhull_t +{ + byte hullCount; // only bytes allowed see above + byte pad[3]; + + inline const packedhull_t *GetPackedHull( int hullIndex ) const + { + Assert(hullIndex<hullCount); + return ((const packedhull_t *)(this+1)) + hullIndex; + } + inline const packedtriangle_t *GetPackedTriangles( int hullIndex ) const + { + const packedhull_t *pHull = GetPackedHull(0); + // the first triangle is immediately following the memory for the packed hulls + const byte *pStart = reinterpret_cast<const byte *>(GetPackedHull(0) + hullCount); + for ( int i = 0; i < hullIndex; i++ ) + { + pStart += pHull[i].DataSize(); + } + return reinterpret_cast<const packedtriangle_t *>(pStart); + } + inline const packededge_t *GetPackedEdges( int hullIndex ) const + { + return reinterpret_cast<const packededge_t *>(GetPackedTriangles(hullIndex) + GetPackedHull(hullIndex)->triangleCount); + } + inline size_t TotalSize() const + { + size_t size = sizeof(*this) + sizeof(packedhull_t) * hullCount; + for ( int i = 0; i < hullCount; i++ ) + { + size += GetPackedHull(i)->DataSize(); + } + return size; + + } +}; + +#pragma pack() +// end +// NOTE: EACH ELEMENT OF THE ABOVE STRUCTS MUST BE 8-bits OR YOU HAVE TO WRITE SWAPPING CODE FOR +// THE X360 IMPLEMENTATION. THERE IS NO SUCH CODE AS OF NOW. + + +// These statics are grouped in a class so they can be friends of IVP_Compact_Ledge and access its private data +class CVPhysicsVirtualMeshWriter +{ +public: + // init a 2-sided triangle ledge + static void InitTwoSidedTriangleLege( triangleledge_t *pOut, const IVP_Compact_Poly_Point *pPoints, int v0, int v1, int v2, int materialIndex ); + + static virtualmeshhull_t *CreatePackedHullFromLedges( const virtualmeshlist_t &list, const IVP_Compact_Ledge **pLedges, int ledgeCount ); + static void UnpackCompactLedgeFromHull( IVP_Compact_Ledge *pLedge, int materialIndex, const IVP_Compact_Poly_Point *pPointList, const virtualmeshhull_t *pHullHeader, int hullIndex, bool isVirtualLedge ); + static void DestroyPackedHull( virtualmeshhull_t *pHull ); + static bool LedgeCanBePacked(const IVP_Compact_Ledge *pLedge, const virtualmeshlist_t &list); + static unsigned int UnpackLedgeListFromHull( byte *pOut, virtualmeshhull_t *pHull, IVP_Compact_Poly_Point *pPoints ); +}; + +#endif // LEDGEWRITER_H diff --git a/vphysics/linear_solver.cpp b/vphysics/linear_solver.cpp new file mode 100644 index 0000000..66e36ea --- /dev/null +++ b/vphysics/linear_solver.cpp @@ -0,0 +1,113 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// + +#include "tier0/platform.h" +#include "linear_solver.h" +#include <stdio.h> +#include <string.h> +#include <math.h> + +// BUGBUG: Remove/tune this number somehow!!! +#define DET_EPSILON 1e-6f + + +// assumes square matrix! +// ONLY SUPPORTS 2x2, 3x3, and 4x4 +float Det( float *matrix, int rows ) +{ + if ( rows == 2 ) + { + return matrix[0]*matrix[3] - matrix[1]*matrix[2]; + } + if ( rows == 3 ) + { + return matrix[0]*matrix[4]*matrix[8] - matrix[0]*matrix[7]*matrix[5] - matrix[1]*matrix[3]*matrix[8] + + matrix[1]*matrix[5]*matrix[6] + matrix[2]*matrix[3]*matrix[7] - matrix[2]*matrix[4]*matrix[6]; + } + + // ERROR no more than 4x4 + if ( rows != 4 ) + return 0; + + // UNDONE: Generalize this to NxN + float tmp[9]; + float det = 0.f; + // do 4 3x3 dets + for ( int i = 0; i < 4; i++ ) + { + // develop on row 0 + int out = 0; + for ( int j = 1; j < 4; j++ ) + { + // iterate each column and + for ( int k = 0; k < 4; k++ ) + { + if ( k == i ) + continue; + tmp[out] = matrix[(j*rows)+k]; + out++; + } + } + if ( i & 1 ) + { + det -= matrix[i]*Det(tmp,3); + } + else + { + det += matrix[i]*Det(tmp,3); + } + } + + return det; +} + +float *SolveCramer( const float *matrix, int rows, int columns ) +{ + // max 4 equations, 4 unknowns (until determinant routine is more general) + float tmpMain[16*16], tmpSub[16*16]; + static float solution[16]; + + int i, j; + + if ( rows > 4 || columns > 5 ) + { + return NULL; + } + + + int outCol = columns - 1; + // copy out the square matrix + for ( i = 0; i < rows; i++ ) + { + memcpy( tmpMain + (i*outCol), matrix + i*columns, sizeof(float)*outCol ); + } + + float detMain = Det( tmpMain, rows ); + + // probably degenerate! + if ( fabs(detMain) < DET_EPSILON ) + { + return NULL; + } + + for ( i = 0; i < rows; i++ ) + { + // copy the square matrix + memcpy( tmpSub, tmpMain, sizeof(float)*rows*rows ); + + // copy the column of constants over the row + for ( j = 0; j < rows; j++ ) + { + tmpSub[i+j*outCol] = matrix[j*columns+columns-1]; + } + float det = Det( tmpSub, rows ); + solution[i] = det / detMain; + } + + return solution; +} diff --git a/vphysics/linear_solver.h b/vphysics/linear_solver.h new file mode 100644 index 0000000..1995ed9 --- /dev/null +++ b/vphysics/linear_solver.h @@ -0,0 +1,22 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef LINEAR_SOLVER_H +#define LINEAR_SOLVER_H +#ifdef _WIN32 +#pragma once +#endif + + +// Take the determinant of a matrix. +// NOTE: ONLY SUPPORTS 2x2, 3x3, and 4x4 +extern float Det( float *matrix, int rows ); + +// solve a system of linear equations using Cramer's rule (only supports up to 4 variables due to limits on Det()) +extern float *SolveCramer( const float *matrix, int rows, int columns ); + +#endif // LINEAR_SOLVER_H diff --git a/vphysics/main.cpp b/vphysics/main.cpp new file mode 100644 index 0000000..eda9a95 --- /dev/null +++ b/vphysics/main.cpp @@ -0,0 +1,242 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" +#include "interface.h" +#include "vphysics/object_hash.h" +#include "vphysics/collision_set.h" +#include "tier1/tier1.h" +#include "ivu_vhash.hxx" + + + +#if defined(_WIN32) && !defined(_X360) +#define WIN32_LEAN_AND_MEAN +#include <windows.h> +#endif // _WIN32 && !_X360 + +#include "vphysics_interfaceV30.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +static void ivu_string_print_function( const char *str ) +{ + Msg("%s", str); +} + +#if defined(_WIN32) && !defined(_XBOX) +//HMODULE gPhysicsDLLHandle; + +#pragma warning (disable:4100) + +BOOL WINAPI DllMain( HINSTANCE hinstDLL, DWORD fdwReason, LPVOID lpvReserved ) +{ + if ( fdwReason == DLL_PROCESS_ATTACH ) + { + ivp_set_message_print_function( ivu_string_print_function ); + + MathLib_Init( 2.2f, 2.2f, 0.0f, 2.0f, false, false, false, false ); + // store out module handle + //gPhysicsDLLHandle = (HMODULE)hinstDLL; + } + else if ( fdwReason == DLL_PROCESS_DETACH ) + { + } + return TRUE; +} + +#endif // _WIN32 + +#ifdef POSIX +void __attribute__ ((constructor)) vphysics_init(void); +void vphysics_init(void) +{ + ivp_set_message_print_function( ivu_string_print_function ); + + MathLib_Init( 2.2f, 2.2f, 0.0f, 2.0f, false, false, false, false ); +} +#endif + + +// simple 32x32 bit array +class CPhysicsCollisionSet : public IPhysicsCollisionSet +{ +public: + ~CPhysicsCollisionSet() {} + CPhysicsCollisionSet() + { + memset( m_bits, 0, sizeof(m_bits) ); + } + void EnableCollisions( int index0, int index1 ) + { + Assert(index0<32&&index1<32); + m_bits[index0] |= 1<<index1; + m_bits[index1] |= 1<<index0; + } + void DisableCollisions( int index0, int index1 ) + { + Assert(index0<32&&index1<32); + m_bits[index0] &= ~(1<<index1); + m_bits[index1] &= ~(1<<index0); + } + + bool ShouldCollide( int index0, int index1 ) + { + Assert(index0<32&&index1<32); + return (m_bits[index0] & (1<<index1)) ? true : false; + } +private: + unsigned int m_bits[32]; +}; + + +//----------------------------------------------------------------------------- +// Main physics interface +//----------------------------------------------------------------------------- +class CPhysicsInterface : public CTier1AppSystem<IPhysics> +{ +public: + CPhysicsInterface() : m_pCollisionSetHash(NULL) {} + virtual void *QueryInterface( const char *pInterfaceName ); + virtual IPhysicsEnvironment *CreateEnvironment( void ); + virtual void DestroyEnvironment( IPhysicsEnvironment *pEnvironment ); + virtual IPhysicsEnvironment *GetActiveEnvironmentByIndex( int index ); + virtual IPhysicsObjectPairHash *CreateObjectPairHash(); + virtual void DestroyObjectPairHash( IPhysicsObjectPairHash *pHash ); + virtual IPhysicsCollisionSet *FindOrCreateCollisionSet( unsigned int id, int maxElementCount ); + virtual IPhysicsCollisionSet *FindCollisionSet( unsigned int id ); + virtual void DestroyAllCollisionSets(); + +private: + CUtlVector<IPhysicsEnvironment *> m_envList; + CUtlVector<CPhysicsCollisionSet> m_collisionSets; + IVP_VHash_Store *m_pCollisionSetHash; +}; + + +//----------------------------------------------------------------------------- +// Expose singleton +//----------------------------------------------------------------------------- +static CPhysicsInterface g_MainDLLInterface; +IPhysics *g_PhysicsInternal = &g_MainDLLInterface; +EXPOSE_SINGLE_INTERFACE_GLOBALVAR( CPhysicsInterface, IPhysics, VPHYSICS_INTERFACE_VERSION, g_MainDLLInterface ); + + +//----------------------------------------------------------------------------- +// Query interface +//----------------------------------------------------------------------------- +void *CPhysicsInterface::QueryInterface( const char *pInterfaceName ) +{ + // Loading the datacache DLL mounts *all* interfaces + // This includes the backward-compatible interfaces + other vphysics interfaces + CreateInterfaceFn factory = Sys_GetFactoryThis(); // This silly construction is necessary + return factory( pInterfaceName, NULL ); // to prevent the LTCG compiler from crashing. +} + + +//----------------------------------------------------------------------------- +// Implementation of IPhysics +//----------------------------------------------------------------------------- +IPhysicsEnvironment *CPhysicsInterface::CreateEnvironment( void ) +{ + IPhysicsEnvironment *pEnvironment = CreatePhysicsEnvironment(); + m_envList.AddToTail( pEnvironment ); + return pEnvironment; +} + +void CPhysicsInterface::DestroyEnvironment( IPhysicsEnvironment *pEnvironment ) +{ + m_envList.FindAndRemove( pEnvironment ); + delete pEnvironment; +} + +IPhysicsEnvironment *CPhysicsInterface::GetActiveEnvironmentByIndex( int index ) +{ + if ( index < 0 || index >= m_envList.Count() ) + return NULL; + + return m_envList[index]; +} + +IPhysicsObjectPairHash *CPhysicsInterface::CreateObjectPairHash() +{ + return ::CreateObjectPairHash(); +} + +void CPhysicsInterface::DestroyObjectPairHash( IPhysicsObjectPairHash *pHash ) +{ + delete pHash; +} +// holds a cache of these by id. +// NOTE: This is stuffed into vphysics.dll as a sneaky way of sharing the memory between +// client and server in single player. So you can't have different client/server rules. +IPhysicsCollisionSet *CPhysicsInterface::FindOrCreateCollisionSet( unsigned int id, int maxElementCount ) +{ + if ( !m_pCollisionSetHash ) + { + m_pCollisionSetHash = new IVP_VHash_Store(256); + } + Assert( id != 0 ); + Assert( maxElementCount <= 32 ); + if ( maxElementCount > 32 ) + return NULL; + + IPhysicsCollisionSet *pSet = FindCollisionSet( id ); + if ( pSet ) + return pSet; + int index = m_collisionSets.AddToTail(); + m_pCollisionSetHash->add_elem( (void *)id, (void *)(index+1) ); + return &m_collisionSets[index]; +} + +IPhysicsCollisionSet *CPhysicsInterface::FindCollisionSet( unsigned int id ) +{ + if ( m_pCollisionSetHash ) + { + int index = (int)m_pCollisionSetHash->find_elem( (void *)id ); + if ( index > 0 ) + { + Assert( index <= m_collisionSets.Count() ); + if ( index <= m_collisionSets.Count() ) + { + return &m_collisionSets[index-1]; + } + } + } + return NULL; +} + +void CPhysicsInterface::DestroyAllCollisionSets() +{ + m_collisionSets.Purge(); + delete m_pCollisionSetHash; + m_pCollisionSetHash = NULL; +} + + + +// In release build, each of these libraries must contain a symbol that indicates it is also a release build +// You MUST disable this in order to run a release vphysics.dll with a debug library. +// This should not usually be necessary +#if !defined(_DEBUG) && defined(_WIN32) +extern int ivp_physics_lib_is_a_release_build; +extern int ivp_compactbuilder_lib_is_a_release_build; +extern int hk_base_lib_is_a_release_build; +extern int hk_math_lib_is_a_release_build; +extern int havana_constraints_lib_is_a_release_build; + +void DebugTestFunction() +{ + ivp_physics_lib_is_a_release_build = 0; + ivp_compactbuilder_lib_is_a_release_build = 0; + hk_base_lib_is_a_release_build = 0; + hk_math_lib_is_a_release_build = 0; + havana_constraints_lib_is_a_release_build = 0; +} +#endif + diff --git a/vphysics/perftest/perftest.cpp b/vphysics/perftest/perftest.cpp new file mode 100644 index 0000000..18843ab --- /dev/null +++ b/vphysics/perftest/perftest.cpp @@ -0,0 +1,342 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "stdafx.h" +#include "filesystem_tools.h" +#include "KeyValues.h" +#include "physdll.h" +#include "materialsystem/imesh.h" +#include "utlvector.h" + +char g_szAppName[] = "VPhysics perf test"; +bool g_bCaptureOnFocus = false; + +IPhysics *physics = NULL; +IPhysicsCollision *physcollision = NULL; +IPhysicsSurfaceProps *physprops = NULL; +IMaterial *g_materialFlatshaded = NULL; +IMaterial *g_pWireframeMaterial = NULL; +int gKeys[256]; + +const objectparams_t g_PhysDefaultObjectParams = +{ + NULL, + 1.0f, //mass + 1.0f, // inertia + 0.0f, // damping + 0.0f, // rotdamping + 0.05f, // rotIntertiaLimit + "DEFAULT", + NULL,// game data + 0.f, // volume (leave 0 if you don't have one or call physcollision->CollideVolume() to compute it) + 1.0f, // drag coefficient + true,// enable collisions? +}; + + +void AddSurfacepropFile( const char *pFileName, IPhysicsSurfaceProps *pProps, IFileSystem *pFileSystem ) +{ + // Load file into memory + FileHandle_t file = pFileSystem->Open( pFileName, "rb" ); + + if ( file ) + { + int len = pFileSystem->Size( file ); + + // read the file + char *buffer = (char *)stackalloc( len+1 ); + pFileSystem->Read( buffer, len, file ); + pFileSystem->Close( file ); + buffer[len] = 0; + pProps->ParseSurfaceData( pFileName, buffer ); + // buffer is on the stack, no need to free + } +} + +void PhysParseSurfaceData( IPhysicsSurfaceProps *pProps, IFileSystem *pFileSystem ) +{ + const char *SURFACEPROP_MANIFEST_FILE = "scripts/surfaceproperties_manifest.txt"; + KeyValues *manifest = new KeyValues( SURFACEPROP_MANIFEST_FILE ); + if ( manifest->LoadFromFile( pFileSystem, SURFACEPROP_MANIFEST_FILE, "GAME" ) ) + { + for ( KeyValues *sub = manifest->GetFirstSubKey(); sub != NULL; sub = sub->GetNextKey() ) + { + if ( !Q_stricmp( sub->GetName(), "file" ) ) + { + // Add + AddSurfacepropFile( sub->GetString(), pProps, pFileSystem ); + continue; + } + + Warning( "surfaceprops::Init: Manifest '%s' with bogus file type '%s', expecting 'file'\n", + SURFACEPROP_MANIFEST_FILE, sub->GetName() ); + } + } + else + { + Error( "Unable to load manifest file '%s'\n", SURFACEPROP_MANIFEST_FILE ); + } + + manifest->deleteThis(); +} + +struct physics_test_object_t +{ + IPhysicsObject *pPhysics; + ICollisionQuery *pModel; +}; + +struct physicstest_t +{ + IPhysicsEnvironment *physenv; + CUtlVector<physics_test_object_t> list; + + void Clear() + { + physenv->SetQuickDelete( true ); + for ( int i = 0; i < list.Count(); i++ ) + { + physcollision->DestroyQueryModel( list[i].pModel ); + physenv->DestroyObject( list[i].pPhysics ); + } + list.Purge(); + physics->DestroyEnvironment( physenv ); + } + void InitEnvironment() + { + physenv = physics->CreateEnvironment(); + //g_EntityCollisionHash = physics->CreateObjectPairHash(); + physenv->EnableDeleteQueue( true ); + + //physenv->SetCollisionSolver( &g_Collisions ); + //physenv->SetCollisionEventHandler( &g_Collisions ); + //physenv->SetConstraintEventHandler( g_pConstraintEvents ); + //physenv->SetObjectEventHandler( &g_Objects ); + + physenv->SetSimulationTimestep( DEFAULT_TICK_INTERVAL ); // 15 ms per tick + // HL Game gravity, not real-world gravity + physenv->SetGravity( Vector( 0, 0, -600.0f ) ); + physenv->SetAirDensity( 0.5f ); + } + + int AddObject( IPhysicsObject *pObject ) + { + int index = list.AddToTail(); + list[index].pPhysics = pObject; + list[index].pModel = physcollision->CreateQueryModel( (CPhysCollide *)pObject->GetCollide() ); + return index; + } + + void CreateGround( float size ) + { + { + CPhysCollide *pCollide = physcollision->BBoxToCollide( Vector(-size,-size,-24), Vector(size,size,0) ); + objectparams_t params = g_PhysDefaultObjectParams; + IPhysicsObject *pGround = physenv->CreatePolyObjectStatic( pCollide, physprops->GetSurfaceIndex( "default" ), vec3_origin, vec3_angle, ¶ms ); + AddObject( pGround ); + } + + for ( int i = 0; i < 20; i++ ) + { + CPhysCollide *pCollide = physcollision->BBoxToCollide( Vector(-24,-24,-24), Vector(24,24,24) ); + objectparams_t params = g_PhysDefaultObjectParams; + params.mass = 150.0f; + IPhysicsObject *pGround = physenv->CreatePolyObject( pCollide, physprops->GetSurfaceIndex( "default" ), Vector(64*(i%4),64 * (i%5),1024), vec3_angle, ¶ms ); + AddObject( pGround ); + pGround->Wake(); + } + } + + void Explode( const Vector &origin, float force ) + { + for ( int i = 0; i < list.Count(); i++ ) + { + if ( !list[i].pPhysics->IsMoveable() ) + continue; + + Vector pos, dir; + list[i].pPhysics->GetPosition( &pos, NULL ); + dir = pos - origin; + dir.z += 10; + VectorNormalize( dir ); + list[i].pPhysics->ApplyForceCenter( dir * force ); + } + } + void RandomColor( float *color, int key ) + { + static bool first = true; + static colorVec colors[256]; + + if ( first ) + { + int r, g, b; + first = false; + for ( int i = 0; i < 256; i++ ) + { + do + { + r = rand()&255; + g = rand()&255; + b = rand()&255; + } while ( (r+g+b)<256 ); + colors[i].r = r; + colors[i].g = g; + colors[i].b = b; + colors[i].a = 255; + } + } + + int index = key & 255; + color[0] = colors[index].r * (1.f / 255.f); + color[1] = colors[index].g * (1.f / 255.f); + color[2] = colors[index].b * (1.f / 255.f); + color[3] = colors[index].a * (1.f / 255.f); + } + + void DrawObject( ICollisionQuery *pModel, IMaterial *pMaterial, IPhysicsObject *pObject ) + { + matrix3x4_t matrix; + pObject->GetPositionMatrix( &matrix ); + CMatRenderContextPtr pRenderContext(g_MaterialSystemApp.m_pMaterialSystem); + pRenderContext->Bind( pMaterial ); + + int vertIndex = 0; + for ( int i = 0; i < pModel->ConvexCount(); i++ ) + { + float color[4]; + RandomColor( color, i + (int)pObject ); + IMesh* pMatMesh = pRenderContext->GetDynamicMesh( ); + CMeshBuilder meshBuilder; + int triCount = pModel->TriangleCount( i ); + meshBuilder.Begin( pMatMesh, MATERIAL_TRIANGLES, triCount ); + + for ( int j = 0; j < triCount; j++ ) + { + Vector objectSpaceVerts[3]; + pModel->GetTriangleVerts( i, j, objectSpaceVerts ); + + for ( int k = 0; k < 3; k++ ) + { + Vector v; + + VectorTransform (objectSpaceVerts[k], matrix, v); + meshBuilder.Position3fv( v.Base() ); + meshBuilder.Color4fv( color ); + meshBuilder.AdvanceVertex(); + } + } + meshBuilder.End( false, true ); + } + } + + void Draw() + { + for ( int i = 0; i < list.Count(); i++ ) + { + DrawObject( list[i].pModel, g_materialFlatshaded, list[i].pPhysics ); + } + } + void Simulate( float frametime ) + { + physenv->Simulate( frametime ); + } +}; + +physicstest_t staticTest; + +void AppInit( void ) +{ + memset( gKeys, 0, sizeof(gKeys) ); + CreateInterfaceFn physicsFactory = GetPhysicsFactory(); + if (!(physics = (IPhysics *)physicsFactory( VPHYSICS_INTERFACE_VERSION, NULL )) || + !(physcollision = (IPhysicsCollision *)physicsFactory( VPHYSICS_COLLISION_INTERFACE_VERSION, NULL )) || + !(physprops = (IPhysicsSurfaceProps *)physicsFactory( VPHYSICS_SURFACEPROPS_INTERFACE_VERSION, NULL )) ) + { + return; + } + + PhysParseSurfaceData( physprops, g_pFullFileSystem ); + g_materialFlatshaded = g_MaterialSystemApp.m_pMaterialSystem->FindMaterial("debug/debugdrawflatpolygons", TEXTURE_GROUP_OTHER, true); + g_pWireframeMaterial = g_MaterialSystemApp.m_pMaterialSystem->FindMaterial("shadertest/wireframevertexcolor", TEXTURE_GROUP_OTHER); + staticTest.InitEnvironment(); + staticTest.CreateGround( 1024 ); +} + +void FPSControls( float frametime, float mouseDeltaX, float mouseDeltaY, Vector& cameraPosition, QAngle& cameraAngles, float speed ) +{ + cameraAngles[1] -= mouseDeltaX; + cameraAngles[0] -= mouseDeltaY; + + if ( cameraAngles[0] < -85 ) + cameraAngles[0] = -85; + if ( cameraAngles[0] > 85 ) + cameraAngles[0] = 85; + + Vector forward, right, up; + AngleVectors( cameraAngles, &forward, &right, &up ); + + if ( gKeys[ 'W' ] ) + VectorMA( cameraPosition, frametime * speed, forward, cameraPosition ); + if ( gKeys[ 'S' ] ) + VectorMA( cameraPosition, -frametime * speed, forward, cameraPosition ); + if ( gKeys[ 'A' ] ) + VectorMA( cameraPosition, -frametime * speed, right, cameraPosition ); + if ( gKeys[ 'D' ] ) + VectorMA( cameraPosition, frametime * speed, right, cameraPosition ); +} + + +void SetupCamera( Vector& cameraPosition, QAngle& cameraAngles ) +{ + CMatRenderContextPtr pRenderContext(g_MaterialSystemApp.m_pMaterialSystem); + pRenderContext->MatrixMode( MATERIAL_VIEW ); + pRenderContext->LoadIdentity( ); + pRenderContext->Rotate( -90, 1, 0, 0 ); // put Z going up + pRenderContext->Rotate( 90, 0, 0, 1 ); + + pRenderContext->Rotate( -cameraAngles[2], 1, 0, 0); // roll + pRenderContext->Rotate( -cameraAngles[0], 0, 1, 0); // pitch + pRenderContext->Rotate( -cameraAngles[1], 0, 0, 1); // yaw + + pRenderContext->Translate( -cameraPosition[0], -cameraPosition[1], -cameraPosition[2] ); +} + +static Vector cameraPosition = Vector(0,0,128); +static QAngle cameraAngles = vec3_angle; + +void AppRender( float frametime, float mouseDeltaX, float mouseDeltaY ) +{ + FPSControls( frametime, mouseDeltaX, mouseDeltaY, cameraPosition, cameraAngles, 300 ); + SetupCamera( cameraPosition, cameraAngles ); + + staticTest.Simulate( frametime ); + staticTest.Draw(); +} + +void AppExit( void ) +{ + staticTest.Clear(); + + //physics->DestroyObjectPairHash( g_EntityCollisionHash ); + //g_EntityCollisionHash = NULL; + physics->DestroyAllCollisionSets(); +} + +void AppKey( int key, int down ) +{ + gKeys[ key & 255 ] = down; +} + + +void AppChar( int key ) +{ + if ( key == ' ' ) + { + staticTest.Explode( cameraPosition, 150 * 100 ); + } +} + diff --git a/vphysics/perftest/perftest.vpc b/vphysics/perftest/perftest.vpc new file mode 100644 index 0000000..6888662 --- /dev/null +++ b/vphysics/perftest/perftest.vpc @@ -0,0 +1,57 @@ +//----------------------------------------------------------------------------- +// PERFTEST.VPC +// +// Project Script +//----------------------------------------------------------------------------- + +$Macro SRCDIR "..\.." +$Macro OUTBINDIR "$SRCDIR\..\game\bin" + +$Include "$SRCDIR\vpc_scripts\source_exe_win_win32_base.vpc" + +$Configuration +{ + $Compiler + { + $AdditionalIncludeDirectories "$BASE,..\..\utils\common,..\..\utils\matsysapp,." + $PreprocessorDefinitions "$BASE;VECTOR;PROTECTED_THINGS_DISABLE" + } + + $Linker + { +// $AdditionalDependencies "comctl32.lib winmm.lib" + } +} + +$Project "perftest" +{ + $Folder "Source Files" + { + $File "..\..\utils\matsysapp\matsysapp.cpp" + $File "perftest.cpp" + + $Folder "common files" + { +// $File "..\..\utils\common\bsplib.cpp" + $File "..\..\utils\common\cmdlib.cpp" + $File "$SRCDIR\public\filesystem_helpers.cpp" + $File "$SRCDIR\public\filesystem_init.cpp" + $File "..\..\utils\common\filesystem_tools.cpp" + $File "..\..\utils\common\physdll.cpp" + $File "..\..\utils\common\scriplib.cpp" + } + } + + $Folder "Header Files" + { + $File "$SRCDIR\public\vphysics_interface.h" + $File "stdafx.h" + } + + $Folder "Link Libraries" + { +// $DynamicFile "$SRCDIR\lib\public\appframework.lib" + $DynamicFile "$SRCDIR\lib\public\mathlib.lib" + $DynamicFile "$SRCDIR\lib\public\tier2.lib" + } +} diff --git a/vphysics/perftest/stdafx.h b/vphysics/perftest/stdafx.h new file mode 100644 index 0000000..07a49ce --- /dev/null +++ b/vphysics/perftest/stdafx.h @@ -0,0 +1,13 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// + +#include "materialsystem/imaterialsystem.h" +#include "matsysapp.h" +#include "mathlib/mathlib.h" +#include "const.h" +#include "vphysics_interface.h" diff --git a/vphysics/physics_airboat.cpp b/vphysics/physics_airboat.cpp new file mode 100644 index 0000000..1437fa4 --- /dev/null +++ b/vphysics/physics_airboat.cpp @@ -0,0 +1,1796 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: The airboat, a sporty nimble water craft. +// +//=============================================================================// + +#include "cbase.h" +#include "physics_airboat.h" +#include "cmodel.h" +#include <ivp_ray_solver.hxx> + + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +#ifdef _X360 + #define AIRBOAT_STEERING_RATE_MIN 0.000225f + #define AIRBOAT_STEERING_RATE_MAX (10.0f * AIRBOAT_STEERING_RATE_MIN) + #define AIRBOAT_STEERING_INTERVAL 1.5f +#else + #define AIRBOAT_STEERING_RATE_MIN 0.00045f + #define AIRBOAT_STEERING_RATE_MAX (5.0f * AIRBOAT_STEERING_RATE_MIN) + #define AIRBOAT_STEERING_INTERVAL 0.5f +#endif //_X360 + +#define AIRBOAT_ROT_DRAG 0.00004f +#define AIRBOAT_ROT_DAMPING 0.001f + +// Mass-independent thrust values +#define AIRBOAT_THRUST_MAX 11.0f // N / kg +#define AIRBOAT_THRUST_MAX_REVERSE 7.5f // N / kg + +// Mass-independent drag values +#define AIRBOAT_WATER_DRAG_LEFT_RIGHT 0.6f +#define AIRBOAT_WATER_DRAG_FORWARD_BACK 0.005f +#define AIRBOAT_WATER_DRAG_UP_DOWN 0.0025f + +#define AIRBOAT_GROUND_DRAG_LEFT_RIGHT 2.0 +#define AIRBOAT_GROUND_DRAG_FORWARD_BACK 1.0 +#define AIRBOAT_GROUND_DRAG_UP_DOWN 0.8 + +#define AIRBOAT_DRY_FRICTION_SCALE 0.6f // unitless, reduces our friction on all surfaces other than water + +#define AIRBOAT_RAYCAST_DIST 0.35f // m (~14in) +#define AIRBOAT_RAYCAST_DIST_WATER_LOW 0.1f // m (~4in) +#define AIRBOAT_RAYCAST_DIST_WATER_HIGH 0.35f // m (~16in) + +// Amplitude of wave noise. Blend from max to min as speed increases. +#define AIRBOAT_WATER_NOISE_MIN 0.01 // m (~0.4in) +#define AIRBOAT_WATER_NOISE_MAX 0.03 // m (~1.2in) + +// Frequency of wave noise. Blend from min to max as speed increases. +#define AIRBOAT_WATER_FREQ_MIN 1.5 +#define AIRBOAT_WATER_FREQ_MAX 1.5 + +// Phase difference in wave noise between left and right pontoons +// Blend from max to min as speed increases. +#define AIRBOAT_WATER_PHASE_MIN 0.0 // s +#define AIRBOAT_WATER_PHASE_MAX 1.5 // s + + +#define AIRBOAT_GRAVITY 9.81f // m/s2 + +// Pontoon indices +enum +{ + AIRBOAT_PONTOON_FRONT_LEFT = 0, + AIRBOAT_PONTOON_FRONT_RIGHT, + AIRBOAT_PONTOON_REAR_LEFT, + AIRBOAT_PONTOON_REAR_RIGHT, +}; + + +class IVP_Ray_Solver_Template; +class IVP_Ray_Hit; +class IVP_Event_Sim; + + +//----------------------------------------------------------------------------- +// Purpose: Constructor +//----------------------------------------------------------------------------- +class CAirboatFrictionData : public IPhysicsCollisionData +{ +public: + CAirboatFrictionData() + { + m_vecPoint.Init( 0, 0, 0 ); + m_vecNormal.Init( 0, 0, 0 ); + m_vecVelocity.Init( 0, 0, 0 ); + } + + virtual void GetSurfaceNormal( Vector &out ) + { + out = m_vecPoint; + } + + virtual void GetContactPoint( Vector &out ) + { + out = m_vecNormal; + } + + virtual void GetContactSpeed( Vector &out ) + { + out = m_vecVelocity; + } + +public: + Vector m_vecPoint; + Vector m_vecNormal; + Vector m_vecVelocity; +}; + + +//----------------------------------------------------------------------------- +// Purpose: Constructor +//----------------------------------------------------------------------------- +CPhysics_Airboat::CPhysics_Airboat( IVP_Environment *pEnv, const IVP_Template_Car_System *pCarSystem, + IPhysicsGameTrace *pGameTrace ) +{ + InitRaycastCarBody( pCarSystem ); + InitRaycastCarEnvironment( pEnv, pCarSystem ); + InitRaycastCarWheels( pCarSystem ); + InitRaycastCarAxes( pCarSystem ); + + InitAirboat( pCarSystem ); + m_pGameTrace = pGameTrace; + + m_SteeringAngle = 0; + m_bSteeringReversed = false; + + m_flThrust = 0; + + m_bAirborne = false; + m_flAirTime = 0; + m_bWeakJump = false; + + m_flPitchErrorPrev = 0; + m_flRollErrorPrev = 0; +} + + +//----------------------------------------------------------------------------- +// Purpose: Deconstructor +//----------------------------------------------------------------------------- +CPhysics_Airboat::~CPhysics_Airboat() +{ + m_pAirboatBody->get_environment()->get_controller_manager()->remove_controller_from_environment( this, IVP_TRUE ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Setup the car system wheels. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::InitAirboat( const IVP_Template_Car_System *pCarSystem ) +{ + for ( int iWheel = 0; iWheel < pCarSystem->n_wheels; ++iWheel ) + { + m_pWheels[iWheel] = pCarSystem->car_wheel[iWheel]; + m_pWheels[iWheel]->enable_collision_detection( IVP_FALSE ); + } + + CPhysicsObject* pBodyObject = static_cast<CPhysicsObject*>(pCarSystem->car_body->client_data); + + pBodyObject->EnableGravity( false ); + + // We do our own buoyancy simulation. + pBodyObject->SetCallbackFlags( pBodyObject->GetCallbackFlags() & ~CALLBACK_DO_FLUID_SIMULATION ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Get the raycast wheel. +//----------------------------------------------------------------------------- +IPhysicsObject *CPhysics_Airboat::GetWheel( int index ) +{ + Assert( index >= 0 ); + Assert( index < n_wheels ); + + return ( IPhysicsObject* )m_pWheels[index]->client_data; +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::SetWheelFriction( int iWheel, float flFriction ) +{ + change_friction_of_wheel( IVP_POS_WHEEL( iWheel ), flFriction ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Returns an amount to add to the front pontoon raycasts to simulate wave noise. +// Input : nPontoonIndex - Which pontoon we're dealing with (0 or 1). +// flSpeedRatio - Speed as a ratio of max speed [0..1] +//----------------------------------------------------------------------------- +float CPhysics_Airboat::ComputeFrontPontoonWaveNoise( int nPontoonIndex, float flSpeedRatio ) +{ + // Add in sinusoidal noise cause by undulating water. Reduce the amplitude of the noise at higher speeds. + IVP_FLOAT flNoiseScale = RemapValClamped( 1.0 - flSpeedRatio, 0, 1, AIRBOAT_WATER_NOISE_MIN, AIRBOAT_WATER_NOISE_MAX ); + + // Apply a phase shift between left and right pontoons to simulate waves passing under the boat. + IVP_FLOAT flPhaseShift = 0; + if ( flSpeedRatio < 0.3 ) + { + // BUG: this allows a discontinuity in the waveform - use two superimposed sine waves instead? + flPhaseShift = nPontoonIndex * AIRBOAT_WATER_PHASE_MAX; + } + + // Increase the wave frequency as speed increases. + IVP_FLOAT flFrequency = RemapValClamped( flSpeedRatio, 0, 1, AIRBOAT_WATER_FREQ_MIN, AIRBOAT_WATER_FREQ_MAX ); + + //Msg( "Wave amp=%f, freq=%f, phase=%f\n", flNoiseScale, flFrequency, flPhaseShift ); + return flNoiseScale * sin( flFrequency * ( m_pCore->environment->get_current_time().get_seconds() + flPhaseShift ) ); +} + + +//----------------------------------------------------------------------------- +// Purpose:: Convert data to HL2 measurements, and test direction of raycast. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::pre_raycasts_gameside( int nRaycastCount, IVP_Ray_Solver_Template *pRays, + Ray_t *pGameRays, IVP_Raycast_Airboat_Impact *pImpacts ) +{ + IVP_FLOAT flForwardSpeedRatio = clamp( m_vecLocalVelocity.k[2] / 10.0f, 0.f, 1.0f ); + //Msg( "flForwardSpeedRatio = %f\n", flForwardSpeedRatio ); + + IVP_FLOAT flSpeed = ( IVP_FLOAT )m_pCore->speed.real_length(); + IVP_FLOAT flSpeedRatio = clamp( flSpeed / 15.0f, 0.f, 1.0f ); + if ( !m_flThrust ) + { + flForwardSpeedRatio *= 0.5; + } + + // This is a little weird. We adjust the front pontoon ray lengths based on forward velocity, + // but ONLY if both pontoons are in the water, which we won't know until we do the raycast. + // So we do most of the work here, and cache some of the results to use them later. + Vector vecStart[4]; + Vector vecDirection[4]; + Vector vecZero( 0.0f, 0.0f, 0.0f ); + + int nFrontPontoonsInWater = 0; + + int iRaycast; + for ( iRaycast = 0; iRaycast < nRaycastCount; ++iRaycast ) + { + // Setup the ray. + ConvertPositionToHL( pRays[iRaycast].ray_start_point, vecStart[iRaycast] ); + ConvertDirectionToHL( pRays[iRaycast].ray_normized_direction, vecDirection[iRaycast] ); + + float flRayLength = IVP2HL( pRays[iRaycast].ray_length ); + + // Check to see if that point is in water. + pImpacts[iRaycast].bInWater = IVP_FALSE; + if ( m_pGameTrace->VehiclePointInWater( vecStart[iRaycast] ) ) + { + vecDirection[iRaycast].Negate(); + pImpacts[iRaycast].bInWater = IVP_TRUE; + } + + Vector vecEnd = vecStart[iRaycast] + ( vecDirection[iRaycast] * flRayLength ); + + // Adjust the trace if the pontoon is in the water. + if ( m_pGameTrace->VehiclePointInWater( vecEnd ) ) + { + // Reduce the ray length in the water. + pRays[iRaycast].ray_length = AIRBOAT_RAYCAST_DIST_WATER_LOW; + + if ( iRaycast < 2 ) + { + nFrontPontoonsInWater++; + + // Front pontoons. + // Add a little sinusoidal noise to simulate waves. + IVP_FLOAT flNoise = ComputeFrontPontoonWaveNoise( iRaycast, flSpeedRatio ); + pRays[iRaycast].ray_length += flNoise; + } + else + { + // Recalculate the end position in HL coordinates. + flRayLength = IVP2HL( pRays[iRaycast].ray_length ); + vecEnd = vecStart[iRaycast] + ( vecDirection[iRaycast] * flRayLength ); + } + } + + pGameRays[iRaycast].Init( vecStart[iRaycast], vecEnd, vecZero, vecZero ); + } + + // If both front pontoons are in the water, add in a bit of lift proportional to our + // forward speed. We can't do this to only one of the front pontoons because it causes + // some twist if we do. + // FIXME: this does some redundant work (computes the wave noise again) + if ( nFrontPontoonsInWater == 2 ) + { + for ( int i = 0; i < 2; i++ ) + { + // Front pontoons. + // Raise it higher out of the water as we go faster forward. + pRays[i].ray_length = RemapValClamped( flForwardSpeedRatio, 0, 1, AIRBOAT_RAYCAST_DIST_WATER_LOW, AIRBOAT_RAYCAST_DIST_WATER_HIGH ); + + // Add a little sinusoidal noise to simulate waves. + IVP_FLOAT flNoise = ComputeFrontPontoonWaveNoise( i, flSpeedRatio ); + pRays[i].ray_length += flNoise; + + // Recalculate the end position in HL coordinates. + float flRayLength = IVP2HL( pRays[i].ray_length ); + Vector vecEnd = vecStart[i] + ( vecDirection[i] * flRayLength ); + + pGameRays[i].Init( vecStart[i], vecEnd, vecZero, vecZero ); + } + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +float CPhysics_Airboat::GetWaterDepth( Ray_t *pGameRay, IPhysicsObject *pPhysAirboat ) +{ + float flDepth = 0.0f; + + trace_t trace; + + Ray_t waterRay; + Vector vecStart = pGameRay->m_Start; + Vector vecEnd( vecStart.x, vecStart.y, vecStart.z + 1000.0f ); + Vector vecZero( 0.0f, 0.0f, 0.0f ); + waterRay.Init( vecStart, vecEnd, vecZero, vecZero ); + m_pGameTrace->VehicleTraceRayWithWater( waterRay, pPhysAirboat->GetGameData(), &trace ); + + flDepth = 1000.0f * trace.fractionleftsolid; + + return flDepth; +} + + +//----------------------------------------------------------------------------- +// Purpose: Performs traces to figure out what is at each of the raycast points +// and fills out the pImpacts array with that information. +// Input : nRaycastCount - Number of elements in the arrays pointed to by pRays +// and pImpacts. +// pRays - Holds the rays to trace with. +// pImpacts - Receives the trace results. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::do_raycasts_gameside( int nRaycastCount, IVP_Ray_Solver_Template *pRays, + IVP_Raycast_Airboat_Impact *pImpacts ) +{ + Assert( nRaycastCount >= 0 ); + Assert( nRaycastCount <= IVP_RAYCAST_AIRBOAT_MAX_WHEELS ); + + Ray_t gameRays[IVP_RAYCAST_AIRBOAT_MAX_WHEELS]; + pre_raycasts_gameside( nRaycastCount, pRays, gameRays, pImpacts ); + + // Do the raycasts and set impact data. + trace_t trace; + for ( int iRaycast = 0; iRaycast < nRaycastCount; ++iRaycast ) + { + // Trace. + if ( pImpacts[iRaycast].bInWater ) + { + // The start position is underwater. Trace up to find the water surface. + IPhysicsObject *pPhysAirboat = static_cast<IPhysicsObject*>( m_pAirboatBody->client_data ); + m_pGameTrace->VehicleTraceRay( gameRays[iRaycast], pPhysAirboat->GetGameData(), &trace ); + pImpacts[iRaycast].flDepth = GetWaterDepth( &gameRays[iRaycast], pPhysAirboat ); + } + else + { + // Trace down to find the ground or water. + IPhysicsObject *pPhysAirboat = static_cast<IPhysicsObject*>( m_pAirboatBody->client_data ); + m_pGameTrace->VehicleTraceRayWithWater( gameRays[iRaycast], pPhysAirboat->GetGameData(), &trace ); + } + + ConvertPositionToIVP( gameRays[iRaycast].m_Start + gameRays[iRaycast].m_StartOffset, m_CarSystemDebugData.wheelRaycasts[iRaycast][0] ); + ConvertPositionToIVP( gameRays[iRaycast].m_Start + gameRays[iRaycast].m_StartOffset + gameRays[iRaycast].m_Delta, m_CarSystemDebugData.wheelRaycasts[iRaycast][1] ); + m_CarSystemDebugData.wheelRaycastImpacts[iRaycast] = trace.fraction * gameRays[iRaycast].m_Delta.Length(); + + // Set impact data. + pImpacts[iRaycast].bImpactWater = IVP_FALSE; + pImpacts[iRaycast].bImpact = IVP_FALSE; + if ( trace.fraction != 1.0f ) + { + pImpacts[iRaycast].bImpact = IVP_TRUE; + + // Set water surface flag. + pImpacts[iRaycast].flDepth = 0.0f; + if ( trace.contents & MASK_WATER ) + { + pImpacts[iRaycast].bImpactWater = IVP_TRUE; + } + + // Save impact surface data. + ConvertPositionToIVP( trace.endpos, pImpacts[iRaycast].vecImpactPointWS ); + ConvertDirectionToIVP( trace.plane.normal, pImpacts[iRaycast].vecImpactNormalWS ); + + // Save surface properties. + const surfacedata_t *pSurfaceData = physprops->GetSurfaceData( trace.surface.surfaceProps ); + + pImpacts[iRaycast].nSurfaceProps = trace.surface.surfaceProps; + + if (pImpacts[iRaycast].vecImpactNormalWS.k[1] < -0.707) + { + // dampening is 1/t, where t is how long it takes to come to a complete stop + pImpacts[iRaycast].flDampening = pSurfaceData->physics.dampening; + pImpacts[iRaycast].flFriction = pSurfaceData->physics.friction; + } + else + { + // This surface is too vertical -- no friction or damping from it. + pImpacts[iRaycast].flDampening = pSurfaceData->physics.dampening; + pImpacts[iRaycast].flFriction = pSurfaceData->physics.friction; + } + } + } +} + + +//----------------------------------------------------------------------------- +// Purpose: Entry point for airboat simulation. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::do_simulation_controller( IVP_Event_Sim *pEventSim, IVP_U_Vector<IVP_Core> * ) +{ + IVP_Ray_Solver_Template raySolverTemplates[IVP_RAYCAST_AIRBOAT_MAX_WHEELS]; + IVP_Raycast_Airboat_Impact impacts[IVP_RAYCAST_AIRBOAT_MAX_WHEELS]; + + // Cache some data into members here so we only do the work once. + m_pCore = m_pAirboatBody->get_core(); + const IVP_U_Matrix *matWorldFromCore = m_pCore->get_m_world_f_core_PSI(); + + // Cache the speed. + m_flSpeed = ( IVP_FLOAT )m_pCore->speed.real_length(); + + // Cache the local velocity vector. + matWorldFromCore->vimult3(&m_pCore->speed, &m_vecLocalVelocity); + + // Raycasts. + PreRaycasts( raySolverTemplates, matWorldFromCore, impacts ); + do_raycasts_gameside( n_wheels, raySolverTemplates, impacts ); + if ( !PostRaycasts( raySolverTemplates, matWorldFromCore, impacts ) ) + return; + + UpdateAirborneState( impacts, pEventSim ); + + // Enumerate the controllers attached to us. + //for (int i = m_pCore->controllers_of_core.len() - 1; i >= 0; i--) + //{ + // IVP_Controller *pController = m_pCore->controllers_of_core.element_at(i); + //} + + // Pontoons. Buoyancy or ground impacts. + DoSimulationPontoons( impacts, pEventSim ); + + // Drag due to water and ground friction. + DoSimulationDrag( impacts, pEventSim ); + + // Turbine (fan). + DoSimulationTurbine( pEventSim ); + + // Steering. + DoSimulationSteering( pEventSim ); + + // Anti-pitch. + DoSimulationKeepUprightPitch( impacts, pEventSim ); + + // Anti-roll. + DoSimulationKeepUprightRoll( impacts, pEventSim ); + + // Additional gravity based on speed. + DoSimulationGravity( pEventSim ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Initialize the rays to be cast from the vehicle wheel positions to +// the "ground." +// Input : pRaySolverTemplates - +// matWorldFromCore - +// pImpacts - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::PreRaycasts( IVP_Ray_Solver_Template *pRaySolverTemplates, + const IVP_U_Matrix *matWorldFromCore, + IVP_Raycast_Airboat_Impact *pImpacts ) +{ + int nPontoonPoints = n_wheels; + for ( int iPoint = 0; iPoint < nPontoonPoints; ++iPoint ) + { + IVP_Raycast_Airboat_Wheel *pPontoonPoint = get_wheel( IVP_POS_WHEEL( iPoint ) ); + if ( pPontoonPoint ) + { + // Fill the in the ray solver template for the current wheel. + IVP_Ray_Solver_Template &raySolverTemplate = pRaySolverTemplates[iPoint]; + + // Transform the wheel "start" position from vehicle core-space to world-space. This is + // the raycast starting position. + matWorldFromCore->vmult4( &pPontoonPoint->raycast_start_cs, &raySolverTemplate.ray_start_point ); + + // Transform the shock (spring) direction from vehicle core-space to world-space. This is + // the raycast direction. + matWorldFromCore->vmult3( &pPontoonPoint->raycast_dir_cs, &pImpacts[iPoint].raycast_dir_ws ); + raySolverTemplate.ray_normized_direction.set( &pImpacts[iPoint].raycast_dir_ws ); + + // Set the length of the ray cast. + raySolverTemplate.ray_length = AIRBOAT_RAYCAST_DIST; + + // Set the ray solver template flags. This defines which objects you wish to + // collide against in the physics environment. + raySolverTemplate.ray_flags = IVP_RAY_SOLVER_ALL; + } + } +} + + +//----------------------------------------------------------------------------- +// Purpose: Determines whether we are airborne and whether we just performed a +// weak or strong jump. Weak jumps are jumps at below a threshold speed, +// and disable the turbine and pitch controller. +// Input : pImpacts - +// Output : Returns true on success, false on failure. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::UpdateAirborneState( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ) +{ + int nCount = CountSurfaceContactPoints(pImpacts); + if (!nCount) + { + if (!m_bAirborne) + { + m_bAirborne = true; + m_flAirTime = 0; + + IVP_FLOAT flSpeed = ( IVP_FLOAT )m_pCore->speed.real_length(); + if (flSpeed < 11.0f) + { + //Msg("*** WEAK JUMP at %f!!!\n", flSpeed); + m_bWeakJump = true; + } + else + { + //Msg("Strong JUMP at %f\n", flSpeed); + } + } + else + { + m_flAirTime += pEventSim->delta_time; + } + } + else + { + m_bAirborne = false; + m_bWeakJump = false; + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +bool CPhysics_Airboat::PostRaycasts( IVP_Ray_Solver_Template *pRaySolverTemplates, const IVP_U_Matrix *matWorldFromCore, + IVP_Raycast_Airboat_Impact *pImpacts ) +{ + bool bReturn = true; + + int nPontoonPoints = n_wheels; + for( int iPoint = 0; iPoint < nPontoonPoints; ++iPoint ) + { + // Get data at raycast position. + IVP_Raycast_Airboat_Wheel *pPontoonPoint = get_wheel( IVP_POS_WHEEL( iPoint ) ); + IVP_Raycast_Airboat_Impact *pImpact = &pImpacts[iPoint]; + IVP_Ray_Solver_Template *pRaySolver = &pRaySolverTemplates[iPoint]; + if ( !pPontoonPoint || !pImpact || !pRaySolver ) + continue; + + // Copy the ray length back, it may have changed. + pPontoonPoint->raycast_length = pRaySolver->ray_length; + + // Test for inverted raycast direction. + if ( pImpact->bInWater ) + { + pImpact->raycast_dir_ws.set_multiple( &pImpact->raycast_dir_ws, -1 ); + } + + // Impact. + if ( pImpact->bImpact ) + { + // Save impact distance. + IVP_U_Point vecDelta; + vecDelta.subtract( &pImpact->vecImpactPointWS, &pRaySolver->ray_start_point ); + pPontoonPoint->raycast_dist = vecDelta.real_length(); + + // Get the inverse portion of the surface normal in the direction of the ray cast (shock - used in the shock simulation code for the sign + // and percentage of force applied to the shock). + pImpact->inv_normal_dot_dir = 1.1f / ( IVP_Inline_Math::fabsd( pImpact->raycast_dir_ws.dot_product( &pImpact->vecImpactNormalWS ) ) + 0.1f ); + + // Set the wheel friction - ground friction (if any) + wheel friction. + pImpact->friction_value = pImpact->flFriction * pPontoonPoint->friction_of_wheel; + } + // No impact. + else + { + pPontoonPoint->raycast_dist = pPontoonPoint->raycast_length; + + pImpact->inv_normal_dot_dir = 1.0f; + pImpact->moveable_object_hit_by_ray = NULL; + pImpact->vecImpactNormalWS.set_multiple( &pImpact->raycast_dir_ws, -1 ); + pImpact->friction_value = 1.0f; + } + + // Set the new wheel position (the impact point or the full ray distance). Make this from the wheel not the ray trace position. + pImpact->vecImpactPointWS.add_multiple( &pRaySolver->ray_start_point, &pImpact->raycast_dir_ws, pPontoonPoint->raycast_dist ); + + // Get the speed (velocity) at the impact point. + m_pCore->get_surface_speed_ws( &pImpact->vecImpactPointWS, &pImpact->surface_speed_wheel_ws ); + pImpact->projected_surface_speed_wheel_ws.set_orthogonal_part( &pImpact->surface_speed_wheel_ws, &pImpact->vecImpactNormalWS ); + + matWorldFromCore->vmult3( &pPontoonPoint->axis_direction_cs, &pImpact->axis_direction_ws ); + pImpact->projected_axis_direction_ws.set_orthogonal_part( &pImpact->axis_direction_ws, &pImpact->vecImpactNormalWS ); + if ( pImpact->projected_axis_direction_ws.normize() == IVP_FAULT ) + { + DevMsg( "CPhysics_Airboat::do_simulation_controller projected_axis_direction_ws.normize failed\n" ); + bReturn = false; + } + } + + return bReturn; +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationPontoons( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ) +{ + int nPontoonPoints = n_wheels; + for ( int iPoint = 0; iPoint < nPontoonPoints; ++iPoint ) + { + IVP_Raycast_Airboat_Wheel *pPontoonPoint = get_wheel( IVP_POS_WHEEL( iPoint ) ); + if ( !pPontoonPoint ) + continue; + + if ( pImpacts[iPoint].bImpact ) + { + DoSimulationPontoonsGround( pPontoonPoint, &pImpacts[iPoint], pEventSim ); + } + else if ( pImpacts[iPoint].bInWater ) + { + DoSimulationPontoonsWater( pPontoonPoint, &pImpacts[iPoint], pEventSim ); + } + } +} + + +//----------------------------------------------------------------------------- +// Purpose: Handle pontoons on ground. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationPontoonsGround( IVP_Raycast_Airboat_Wheel *pPontoonPoint, + IVP_Raycast_Airboat_Impact *pImpact, IVP_Event_Sim *pEventSim ) +{ + // Check to see if we hit anything, otherwise the no force on this point. + IVP_DOUBLE flDiff = pPontoonPoint->raycast_dist - pPontoonPoint->raycast_length; + if ( flDiff >= 0 ) + return; + + IVP_FLOAT flSpringConstant, flSpringRelax, flSpringCompress; + flSpringConstant = pPontoonPoint->spring_constant; + flSpringRelax = pPontoonPoint->spring_damp_relax; + flSpringCompress = pPontoonPoint->spring_damp_compress; + + IVP_DOUBLE flForce = -flDiff * flSpringConstant; + IVP_FLOAT flInvNormalDotDir = clamp(pImpact->inv_normal_dot_dir, 0.0f, 3.0f); + flForce *= flInvNormalDotDir; + + IVP_U_Float_Point vecSpeedDelta; + vecSpeedDelta.subtract( &pImpact->projected_surface_speed_wheel_ws, &pImpact->surface_speed_wheel_ws ); + + IVP_DOUBLE flSpeed = vecSpeedDelta.dot_product( &pImpact->raycast_dir_ws ); + if ( flSpeed > 0 ) + { + flForce -= flSpringRelax * flSpeed; + } + else + { + flForce -= flSpringCompress * flSpeed; + } + + if ( flForce < 0 ) + { + flForce = 0.0f; + } + + // NOTE: Spring constants are all mass-independent, so no need to multiply by mass here. + IVP_DOUBLE flImpulse = flForce * pEventSim->delta_time; + + IVP_U_Float_Point vecImpulseWS; + vecImpulseWS.set_multiple( &pImpact->vecImpactNormalWS, flImpulse ); + m_pCore->push_core_ws( &pImpact->vecImpactPointWS, &vecImpulseWS ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Handle pontoons on water. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationPontoonsWater( IVP_Raycast_Airboat_Wheel *pPontoonPoint, + IVP_Raycast_Airboat_Impact *pImpact, IVP_Event_Sim *pEventSim ) +{ + #define AIRBOAT_BUOYANCY_SCALAR 1.6f + #define PONTOON_AREA_2D 2.8f // 2 pontoons x 16 in x 136 in = 4352 sq inches = 2.8 sq meters + #define PONTOON_HEIGHT 0.41f // 16 inches high = 0.41 meters + + float flDepth = clamp( pImpact->flDepth, 0.f, PONTOON_HEIGHT ); + //Msg("depth: %f\n", pImpact->flDepth); + + // Depth is in inches, so multiply by 0.0254 meters/inch + IVP_FLOAT flSubmergedVolume = PONTOON_AREA_2D * flDepth * 0.0254; + + // Buoyancy forces are equal to the mass of the water displaced, which is 1000 kg/m^3 + // There are 4 pontoon points, so each one can exert 1/4th of the total buoyancy force. + IVP_FLOAT flForce = AIRBOAT_BUOYANCY_SCALAR * 0.25f * m_pCore->get_mass() * flSubmergedVolume * 1000.0f; + IVP_DOUBLE flImpulse = flForce * pEventSim->delta_time; + + IVP_U_Float_Point vecImpulseWS; + vecImpulseWS.set( 0, -1, 0 ); + vecImpulseWS.mult( flImpulse ); + m_pCore->push_core_ws( &pImpact->vecImpactPointWS, &vecImpulseWS ); + +// Vector vecPoint; +// Vector vecDir(0, 0, 1); +// +// ConvertPositionToHL( pImpact->vecImpactPointWS, vecPoint ); +// CPhysicsEnvironment *pEnv = (CPhysicsEnvironment *)m_pAirboatBody->get_core()->environment->client_data; +// IVPhysicsDebugOverlay *debugoverlay = pEnv->GetDebugOverlay(); +// debugoverlay->AddLineOverlay(vecPoint, vecPoint + vecDir * 128, 255, 0, 255, false, 10.0 ); +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::PerformFrictionNotification( float flEliminatedEnergy, float dt, int nSurfaceProp, IPhysicsCollisionData *pCollisionData ) +{ + CPhysicsObject *pPhysAirboat = static_cast<CPhysicsObject*>( m_pAirboatBody->client_data ); + if ( ( pPhysAirboat->CallbackFlags() & CALLBACK_GLOBAL_FRICTION ) == 0 ) + return; + + IPhysicsCollisionEvent *pEventHandler = pPhysAirboat->GetVPhysicsEnvironment()->GetCollisionEventHandler(); + if ( !pEventHandler ) + return; + + // scrape with an estimate for the energy per unit mass + // This assumes that the game is interested in some measure of vibration + // for sound effects. This also assumes that more massive objects require + // more energy to vibrate. + flEliminatedEnergy *= dt / pPhysAirboat->GetMass(); + if ( flEliminatedEnergy > 0.05f ) + { + pEventHandler->Friction( pPhysAirboat, flEliminatedEnergy, pPhysAirboat->GetMaterialIndexInternal(), nSurfaceProp, pCollisionData ); + } +} + + +//----------------------------------------------------------------------------- +// Purpose: Drag due to water and ground friction. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationDrag( IVP_Raycast_Airboat_Impact *pImpacts, + IVP_Event_Sim *pEventSim ) +{ + const IVP_U_Matrix *matWorldFromCore = m_pCore->get_m_world_f_core_PSI(); + IVP_FLOAT flSpeed = ( IVP_FLOAT )m_pCore->speed.real_length(); + + // Used to make airboat sliding sounds + CAirboatFrictionData frictionData; + ConvertDirectionToHL( m_pCore->speed, frictionData.m_vecVelocity ); + + // Count the pontoons in the water. + int nPontoonPoints = n_wheels; + int nPointsInWater = 0; + int nPointsOnGround = 0; + float flGroundFriction = 0; + float flAverageDampening = 0.0f; + int *pSurfacePropCount = (int *)stackalloc( n_wheels * sizeof(int) ); + int *pSurfaceProp = (int *)stackalloc( n_wheels * sizeof(int) ); + memset( pSurfacePropCount, 0, n_wheels * sizeof(int) ); + memset( pSurfaceProp, 0xFF, n_wheels * sizeof(int) ); + int nSurfacePropCount = 0; + int nMaxSurfacePropIdx = 0; + for( int iPoint = 0; iPoint < nPontoonPoints; ++iPoint ) + { + // Get data at raycast position. + IVP_Raycast_Airboat_Impact *pImpact = &pImpacts[iPoint]; + if ( !pImpact || !pImpact->bImpact ) + continue; + + if ( pImpact->bImpactWater ) + { + flAverageDampening += pImpact->flDampening; + nPointsInWater++; + } + else + { + flGroundFriction += pImpact->flFriction; + nPointsOnGround++; + + // This logic is used to determine which surface prop we hit the most. + int i; + for ( i = 0; i < nSurfacePropCount; ++i ) + { + if ( pSurfaceProp[i] == pImpact->nSurfaceProps ) + break; + } + + if ( i == nSurfacePropCount ) + { + ++nSurfacePropCount; + } + pSurfaceProp[i] = pImpact->nSurfaceProps; + if ( ++pSurfacePropCount[i] > pSurfacePropCount[nMaxSurfacePropIdx] ) + { + nMaxSurfacePropIdx = i; + } + + Vector frictionPoint, frictionNormal; + ConvertPositionToHL( pImpact->vecImpactPointWS, frictionPoint ); + ConvertDirectionToHL( pImpact->vecImpactNormalWS, frictionNormal ); + frictionData.m_vecPoint += frictionPoint; + frictionData.m_vecNormal += frictionNormal; + } + } + + int nSurfaceProp = pSurfaceProp[nMaxSurfacePropIdx]; + if ( nPointsOnGround > 0 ) + { + frictionData.m_vecPoint /= nPointsOnGround; + frictionData.m_vecNormal /= nPointsOnGround; + VectorNormalize( frictionData.m_vecNormal ); + } + + if ( nPointsInWater > 0 ) + { + flAverageDampening /= nPointsInWater; + } + + //IVP_FLOAT flDebugSpeed = ( IVP_FLOAT )m_pCore->speed.real_length(); + //Msg("(water=%d/land=%d) speed=%f (%f %f %f)\n", nPointsInWater, nPointsOnGround, flDebugSpeed, vecAirboatDirLS.k[0], vecAirboatDirLS.k[1], vecAirboatDirLS.k[2]); + + if ( nPointsInWater ) + { + // Apply the drag force opposite to the direction of motion in local space. + IVP_U_Float_Point vecAirboatNegDirLS; + vecAirboatNegDirLS.set_negative( &m_vecLocalVelocity ); + + // Water drag is directional -- the pontoons resist left/right motion much more than forward/back. + IVP_U_Float_Point vecDragLS; + vecDragLS.set( AIRBOAT_WATER_DRAG_LEFT_RIGHT * vecAirboatNegDirLS.k[0], + AIRBOAT_WATER_DRAG_UP_DOWN * vecAirboatNegDirLS.k[1], + AIRBOAT_WATER_DRAG_FORWARD_BACK * vecAirboatNegDirLS.k[2] ); + + vecDragLS.mult( flSpeed * m_pCore->get_mass() * pEventSim->delta_time ); + // dvs TODO: apply flAverageDampening here + + // Convert the drag force to world space and apply the drag. + IVP_U_Float_Point vecDragWS; + matWorldFromCore->vmult3(&vecDragLS, &vecDragWS); + m_pCore->center_push_core_multiple_ws( &vecDragWS ); + } + + // + // Calculate ground friction drag: + // + if ( nPointsOnGround && ( flSpeed > 0 )) + { + // Calculate the average friction across all contact points. + flGroundFriction /= (float)nPointsOnGround; + + // Apply the drag force opposite to the direction of motion. + IVP_U_Float_Point vecAirboatNegDir; + vecAirboatNegDir.set_negative( &m_pCore->speed ); + + IVP_FLOAT flFrictionDrag = m_pCore->get_mass() * AIRBOAT_GRAVITY * AIRBOAT_DRY_FRICTION_SCALE * flGroundFriction; + flFrictionDrag /= flSpeed; + + IPhysicsObject *pPhysAirboat = static_cast<IPhysicsObject*>( m_pAirboatBody->client_data ); + float flEliminatedEnergy = pPhysAirboat->GetEnergy(); + + // Apply the drag force opposite to the direction of motion in local space. + IVP_U_Float_Point vecAirboatNegDirLS; + vecAirboatNegDirLS.set_negative( &m_vecLocalVelocity ); + + // Ground drag is directional -- the pontoons resist left/right motion much more than forward/back. + IVP_U_Float_Point vecDragLS; + vecDragLS.set( AIRBOAT_GROUND_DRAG_LEFT_RIGHT * vecAirboatNegDirLS.k[0], + AIRBOAT_GROUND_DRAG_UP_DOWN * vecAirboatNegDirLS.k[1], + AIRBOAT_GROUND_DRAG_FORWARD_BACK * vecAirboatNegDirLS.k[2] ); + + vecDragLS.mult( flFrictionDrag * pEventSim->delta_time ); + // dvs TODO: apply flAverageDampening here + + // Convert the drag force to world space and apply the drag. + IVP_U_Float_Point vecDragWS; + matWorldFromCore->vmult3(&vecDragLS, &vecDragWS); + m_pCore->center_push_core_multiple_ws( &vecDragWS ); + + // Figure out how much energy was eliminated by friction. + flEliminatedEnergy -= pPhysAirboat->GetEnergy(); + PerformFrictionNotification( flEliminatedEnergy, pEventSim->delta_time, nSurfaceProp, &frictionData ); + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationTurbine( IVP_Event_Sim *pEventSim ) +{ + // Reduce the turbine power during weak jumps to avoid unrealistic air control. + // Also, reduce reverse thrust while airborne. + float flThrust = m_flThrust; + if ((m_bWeakJump) || (m_bAirborne && (flThrust < 0))) + { + flThrust *= 0.5; + } + + // Get the forward vector in world-space. + IVP_U_Float_Point vecForwardWS; + const IVP_U_Matrix *matWorldFromCore = m_pCore->get_m_world_f_core_PSI(); + matWorldFromCore->get_col( IVP_COORDINATE_INDEX( index_z ), &vecForwardWS ); + + //Msg("thrust: %f\n", m_flThrust); + if ( ( vecForwardWS.k[1] < -0.5 ) && ( flThrust > 0 ) ) + { + // Driving up a slope. Reduce upward thrust to prevent ludicrous climbing of steep surfaces. + float flFactor = 1 + vecForwardWS.k[1]; + //Msg("FWD: y=%f, factor=%f\n", vecForwardWS.k[1], flFactor); + flThrust *= flFactor; + } + else if ( ( vecForwardWS.k[1] > 0.5 ) && ( flThrust < 0 ) ) + { + // Reversing up a slope. Reduce upward thrust to prevent ludicrous climbing of steep surfaces. + float flFactor = 1 - vecForwardWS.k[1]; + //Msg("REV: y=%f, factor=%f\n", vecForwardWS.k[1], flFactor); + flThrust *= flFactor; + } + + // Forward (Front/Back) force + IVP_U_Float_Point vecImpulse; + vecImpulse.set_multiple( &vecForwardWS, flThrust * m_pCore->get_mass() * pEventSim->delta_time ); + + m_pCore->center_push_core_multiple_ws( &vecImpulse ); +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationSteering( IVP_Event_Sim *pEventSim ) +{ + // Calculate the steering direction: forward or reverse. + // Don't mess with the steering direction while we're steering, unless thrust is applied. + // This prevents the steering from reversing because we started drifting backwards. + if ( ( m_SteeringAngle == 0 ) || ( m_flThrust != 0 ) ) + { + if ( !m_bAnalogSteering ) + { + // If we're applying reverse thrust, steering is always reversed. + if ( m_flThrust < 0 ) + { + m_bSteeringReversed = true; + } + // Else if we are applying forward thrust or moving forward, use forward steering. + else if ( ( m_flThrust > 0 ) || ( m_vecLocalVelocity.k[2] > 0 ) ) + { + m_bSteeringReversed = false; + } + } + else + { + // Create a dead zone through the middle of the joystick where we don't reverse thrust. + // If we're applying reverse thrust, steering is always reversed. + if ( m_flThrust < -2.0f ) + { + m_bSteeringReversed = true; + } + // Else if we are applying forward thrust or moving forward, use forward steering. + else if ( ( m_flThrust > 2.0f ) || ( m_vecLocalVelocity.k[2] > 0 ) ) + { + m_bSteeringReversed = false; + } + } + } + + // Calculate the steering force. + IVP_FLOAT flForceSteering = 0.0f; + if ( fabsf( m_SteeringAngle ) > 0.01 ) + { + // Get the sign of the steering force. + IVP_FLOAT flSteeringSign = m_SteeringAngle < 0.0f ? -1.0f : 1.0f; + if ( m_bSteeringReversed ) + { + flSteeringSign *= -1.0f; + } + + // If we changed steering sign or went from not steering to steering, reset the steer time + // to blend the new steering force in over time. + IVP_FLOAT flPrevSteeringSign = m_flPrevSteeringAngle < 0.0f ? -1.0f : 1.0f; + if ( ( fabs( m_flPrevSteeringAngle ) < 0.01 ) || ( flSteeringSign != flPrevSteeringSign ) ) + { + m_flSteerTime = 0; + } + + float flSteerScale = 0.f; + if ( !m_bAnalogSteering ) + { + // Ramp the steering force up over two seconds. + flSteerScale = RemapValClamped( m_flSteerTime, 0, AIRBOAT_STEERING_INTERVAL, AIRBOAT_STEERING_RATE_MIN, AIRBOAT_STEERING_RATE_MAX ); + } + else // consoles + { + // Analog steering + flSteerScale = RemapValClamped( fabs(m_SteeringAngle), 0, AIRBOAT_STEERING_INTERVAL, AIRBOAT_STEERING_RATE_MIN, AIRBOAT_STEERING_RATE_MAX ); + } + + flForceSteering = flSteerScale * m_pCore->get_mass() * pEventSim->i_delta_time; + flForceSteering *= -flSteeringSign; + + m_flSteerTime += pEventSim->delta_time; + } + + //Msg("steer force=%f\n", flForceSteering); + + m_flPrevSteeringAngle = m_SteeringAngle * ( m_bSteeringReversed ? -1.0 : 1.0 ); + + // Get the sign of the drag forces. + IVP_FLOAT flRotSpeedSign = m_pCore->rot_speed.k[1] < 0.0f ? -1.0f : 1.0f; + + // Apply drag proportional to the square of the angular velocity. + IVP_FLOAT flRotationalDrag = AIRBOAT_ROT_DRAG * m_pCore->rot_speed.k[1] * m_pCore->rot_speed.k[1] * m_pCore->get_mass() * pEventSim->i_delta_time; + flRotationalDrag *= flRotSpeedSign; + + // Apply dampening proportional to angular velocity. + IVP_FLOAT flRotationalDamping = AIRBOAT_ROT_DAMPING * fabs(m_pCore->rot_speed.k[1]) * m_pCore->get_mass() * pEventSim->i_delta_time; + flRotationalDamping *= flRotSpeedSign; + + // Calculate the net rotational force. + IVP_FLOAT flForceRotational = flForceSteering + flRotationalDrag + flRotationalDamping; + + // Apply it. + IVP_U_Float_Point vecRotImpulse; + vecRotImpulse.set( 0, -1, 0 ); + vecRotImpulse.mult( flForceRotational ); + m_pCore->rot_push_core_cs( &vecRotImpulse ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Adds extra gravity unless we are performing a strong jump. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationGravity( IVP_Event_Sim *pEventSim ) +{ + return; + + if ( !m_bAirborne || m_bWeakJump ) + { + IVP_U_Float_Point vecGravity; + vecGravity.set( 0, AIRBOAT_GRAVITY / 2.0f, 0 ); + vecGravity.mult( m_pCore->get_mass() * pEventSim->delta_time ); + m_pCore->center_push_core_multiple_ws( &vecGravity ); + } +} + + +//----------------------------------------------------------------------------- +// Purpose: Returns the number of pontoon raycast points that were found to contact +// the ground or water. +//----------------------------------------------------------------------------- +int CPhysics_Airboat::CountSurfaceContactPoints( IVP_Raycast_Airboat_Impact *pImpacts ) +{ + int nContacts = 0; + int nPontoonPoints = n_wheels; + for ( int iPoint = 0; iPoint < nPontoonPoints; iPoint++ ) + { + // Get data at raycast position. + IVP_Raycast_Airboat_Impact *pImpact = &pImpacts[iPoint]; + if ( !pImpact ) + continue; + + if ( pImpact->bImpact ) + { + nContacts++; + } + } + + return nContacts; +} + + +//----------------------------------------------------------------------------- +// Purpose: Prevents us from nosing down dramatically during jumps, which +// increases our maximum jump distance. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationKeepUprightPitch( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ) +{ + // Disable pitch control during weak jumps. This reduces the unreal 'floaty' sensation. + if (m_bWeakJump) + { + return; + } + + // Reference vector in core space. + // Pitch back by 10 degrees while airborne. + IVP_U_Float_Point vecUpCS; + vecUpCS.set( 0, -cos(DEG2RAD(10)), sin(DEG2RAD(10))); + + // Calculate the goal vector in core space. We will try to align the reference + // vector with the goal vector. + IVP_U_Float_Point vecGoalAxisWS; + vecGoalAxisWS.set( 0, -1, 0 ); + const IVP_U_Matrix *matWorldFromCore = m_pCore->get_m_world_f_core_PSI(); + IVP_U_Float_Point vecGoalAxisCS; + matWorldFromCore->vimult3( &vecGoalAxisWS, &vecGoalAxisCS ); + + // Eliminate roll control + vecGoalAxisCS.k[0] = vecUpCS.k[0]; + vecGoalAxisCS.normize(); + + // Get an axis to rotate around. + IVP_U_Float_Point vecRotAxisCS; + vecRotAxisCS.calc_cross_product( &vecUpCS, &vecGoalAxisCS ); + + // Get the amount that we need to rotate. + // atan2() is well defined, so do a Dot & Cross instead of asin(Cross) + IVP_FLOAT cosine = vecUpCS.dot_product( &vecGoalAxisCS ); + IVP_FLOAT sine = vecRotAxisCS.real_length_plus_normize(); + IVP_FLOAT angle = atan2( sine, cosine ); + + //Msg("angle: %.2f, axis: (%.2f %.2f %.2f)\n", RAD2DEG(angle), vecRotAxisCS.k[0], vecRotAxisCS.k[1], vecRotAxisCS.k[2]); + + // Don't keep upright if any pontoons are contacting a surface. + if ( CountSurfaceContactPoints( pImpacts ) > 0 ) + { + m_flPitchErrorPrev = angle; + return; + } + + // Don't do any correction if we're within 15 degrees of the goal orientation. + //if ( fabs( angle ) < DEG2RAD( 15 ) ) + //{ + // m_flPitchErrorPrev = angle; + // return; + //} + + //Msg("CORRECTING\n"); + + // Generate an angular impulse describing the rotation. + IVP_U_Float_Point vecAngularImpulse; + vecAngularImpulse.set_multiple( &vecRotAxisCS, m_pCore->get_mass() * ( 0.1f * angle + 0.04f * pEventSim->i_delta_time * ( angle - m_flPitchErrorPrev ) ) ); + + // Save the last error value for calculating the derivative. + m_flPitchErrorPrev = angle; + + // Clamp the impulse at a maximum length. + IVP_FLOAT len = vecAngularImpulse.real_length_plus_normize(); + if ( len > ( DEG2RAD( 1.5 ) * m_pCore->get_mass() ) ) + { + len = DEG2RAD( 1.5 ) * m_pCore->get_mass(); + } + vecAngularImpulse.mult( len ); + + // Apply the rotation. + m_pCore->rot_push_core_cs( &vecAngularImpulse ); + +#if DRAW_AIRBOAT_KEEP_UPRIGHT_PITCH_VECTORS + CPhysicsEnvironment *pEnv = (CPhysicsEnvironment *)m_pAirboatBody->get_core()->environment->client_data; + IVPhysicsDebugOverlay *debugoverlay = pEnv->GetDebugOverlay(); + + IVP_U_Float_Point vecPosIVP = m_pCore->get_position_PSI(); + Vector vecPosHL; + ConvertPositionToHL(vecPosIVP, vecPosHL); + + Vector vecGoalAxisHL; + ConvertDirectionToHL(vecGoalAxisWS, vecGoalAxisHL); + + IVP_U_Float_Point vecUpWS; + matWorldFromCore->vmult3( &vecUpCS, &vecUpWS ); + Vector vecCurHL; + ConvertDirectionToHL(vecUpWS, vecCurHL); + + static IVP_FLOAT flLastLen = 0; + IVP_FLOAT flDebugLen = vecAngularImpulse.real_length(); + if ( flLastLen && ( fabs( flDebugLen - flLastLen ) > DEG2RAD( 1 ) * m_pCore->get_mass() ) ) + { + debugoverlay->AddLineOverlay(vecPosHL, vecPosHL + Vector(0, 0, 10) * flDebugLen, 255, 0, 255, false, 100.0 ); + } + else + { + debugoverlay->AddLineOverlay(vecPosHL, vecPosHL + Vector(0, 0, 10) * flDebugLen, 255, 255, 255, false, 100.0 ); + } + debugoverlay->AddLineOverlay(vecPosHL + Vector(0, 0, 10) * flDebugLen, vecPosHL + Vector(0, 0, 10) * flDebugLen + vecGoalAxisHL * 10, 0, 255, 0, false, 100.0 ); + debugoverlay->AddLineOverlay(vecPosHL + Vector(0, 0, 10) * flDebugLen, vecPosHL + Vector(0, 0, 10) * flDebugLen + vecCurHL * 10, 255, 0, 0, false, 100.0 ); + flLastLen = flDebugLen; +#endif +} + + +//----------------------------------------------------------------------------- +// Purpose: Roll stabilizer when airborne. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::DoSimulationKeepUprightRoll( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ) +{ + // Reference vector in core space. + // Pitch back by 10 degrees while airborne. + IVP_U_Float_Point vecUpCS; + vecUpCS.set( 0, -cos(DEG2RAD(10)), sin(DEG2RAD(10))); + + // Calculate the goal vector in core space. We will try to align the reference + // vector with the goal vector. + IVP_U_Float_Point vecGoalAxisWS; + vecGoalAxisWS.set( 0, -1, 0 ); + const IVP_U_Matrix *matWorldFromCore = m_pCore->get_m_world_f_core_PSI(); + IVP_U_Float_Point vecGoalAxisCS; + matWorldFromCore->vimult3( &vecGoalAxisWS, &vecGoalAxisCS ); + + // Eliminate pitch control + vecGoalAxisCS.k[1] = vecUpCS.k[1]; + vecGoalAxisCS.normize(); + + // Get an axis to rotate around. + IVP_U_Float_Point vecRotAxisCS; + vecRotAxisCS.calc_cross_product( &vecUpCS, &vecGoalAxisCS ); + + // Get the amount that we need to rotate. + // atan2() is well defined, so do a Dot & Cross instead of asin(Cross) + IVP_FLOAT cosine = vecUpCS.dot_product( &vecGoalAxisCS ); + IVP_FLOAT sine = vecRotAxisCS.real_length_plus_normize(); + IVP_FLOAT angle = atan2( sine, cosine ); + + //Msg("angle: %.2f, axis: (%.2f %.2f %.2f)\n", RAD2DEG(angle), vecRotAxisCS.k[0], vecRotAxisCS.k[1], vecRotAxisCS.k[2]); + + // Don't keep upright if any pontoons are contacting a surface. + if ( CountSurfaceContactPoints( pImpacts ) > 0 ) + { + m_flRollErrorPrev = angle; + return; + } + + // Don't do any correction if we're within 10 degrees of the goal orientation. + if ( fabs( angle ) < DEG2RAD( 10 ) ) + { + m_flRollErrorPrev = angle; + return; + } + + //Msg("CORRECTING\n"); + + // Generate an angular impulse describing the rotation. + IVP_U_Float_Point vecAngularImpulse; + vecAngularImpulse.set_multiple( &vecRotAxisCS, m_pCore->get_mass() * ( 0.2f * angle + 0.3f * pEventSim->i_delta_time * ( angle - m_flRollErrorPrev ) ) ); + + // Save the last error value for calculating the derivative. + m_flRollErrorPrev = angle; + + // Clamp the impulse at a maximum length. + IVP_FLOAT len = vecAngularImpulse.real_length_plus_normize(); + if ( len > ( DEG2RAD( 2 ) * m_pCore->get_mass() ) ) + { + len = DEG2RAD( 2 ) * m_pCore->get_mass(); + } + vecAngularImpulse.mult( len ); + m_pCore->rot_push_core_cs( &vecAngularImpulse ); + + // Debugging visualization. +#if DRAW_AIRBOAT_KEEP_UPRIGHT_ROLL_VECTORS + CPhysicsEnvironment *pEnv = (CPhysicsEnvironment *)m_pAirboatBody->get_core()->environment->client_data; + IVPhysicsDebugOverlay *debugoverlay = pEnv->GetDebugOverlay(); + + IVP_U_Float_Point vecPosIVP = m_pCore->get_position_PSI(); + Vector vecPosHL; + ConvertPositionToHL(vecPosIVP, vecPosHL); + + Vector vecGoalAxisHL; + ConvertDirectionToHL(vecGoalAxisWS, vecGoalAxisHL); + + IVP_U_Float_Point vecUpWS; + matWorldFromCore->vmult3( &vecUpCS, &vecUpWS ); + Vector vecCurHL; + ConvertDirectionToHL(vecUpWS, vecCurHL); + + static IVP_FLOAT flLastLen = 0; + IVP_FLOAT flDebugLen = vecAngularImpulse.real_length(); + if ( flLastLen && ( fabs( flDebugLen - flLastLen ) > ( DEG2RAD( 0.25 ) * m_pCore->get_mass() ) ) + { + debugoverlay->AddLineOverlay(vecPosHL, vecPosHL + Vector(0, 0, 10) * flDebugLen, 255, 0, 255, false, 100.0 ); + } + else + { + debugoverlay->AddLineOverlay(vecPosHL, vecPosHL + Vector(0, 0, 10) * flDebugLen, 255, 255, 255, false, 100.0 ); + } + debugoverlay->AddLineOverlay(vecPosHL + Vector(0, 0, 10) * flDebugLen, vecPosHL + Vector(0, 0, 10) * flDebugLen + vecGoalAxisHL * 10, 0, 255, 0, false, 100.0 ); + debugoverlay->AddLineOverlay(vecPosHL + Vector(0, 0, 10) * flDebugLen, vecPosHL + Vector(0, 0, 10) * flDebugLen + vecCurHL * 10, 255, 0, 0, false, 100.0 ); + flLastLen = flDebugLen; +#endif +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : wheel_nr - +// s_angle - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::do_steering_wheel(IVP_POS_WHEEL wheel_nr, IVP_FLOAT s_angle) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(wheel_nr); + + wheel->axis_direction_cs.set_to_zero(); + wheel->axis_direction_cs.k[ index_x ] = 1.0f; + wheel->axis_direction_cs.rotate( IVP_COORDINATE_INDEX(index_y), s_angle); +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// spring_constant - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_spring_constant(IVP_POS_WHEEL pos, IVP_FLOAT spring_constant) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + wheel->spring_constant = spring_constant; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// spring_dampening - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_spring_dampening(IVP_POS_WHEEL pos, IVP_FLOAT spring_dampening) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + wheel->spring_damp_relax = spring_dampening; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// spring_dampening - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_spring_dampening_compression(IVP_POS_WHEEL pos, IVP_FLOAT spring_dampening) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + wheel->spring_damp_compress = spring_dampening; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// pre_tension_length - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_spring_pre_tension(IVP_POS_WHEEL pos, IVP_FLOAT pre_tension_length) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + wheel->spring_len = gravity_y_direction * (wheel->distance_orig_hp_to_hp - pre_tension_length); +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// spring_length - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_spring_length(IVP_POS_WHEEL pos, IVP_FLOAT spring_length) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + wheel->spring_len = spring_length; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// torque - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_wheel_torque(IVP_POS_WHEEL pos, IVP_FLOAT torque) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + wheel->torque = torque; + + // Wake the physics object if need be! + m_pAirboatBody->get_environment()->get_controller_manager()->ensure_controller_in_simulation( this ); +} + +IVP_FLOAT CPhysics_Airboat::get_wheel_torque(IVP_POS_WHEEL pos) +{ + return get_wheel(pos)->torque; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Throttle input is -1 to 1. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::update_throttle( IVP_FLOAT flThrottle ) +{ + // Forward + if ( fabs( flThrottle ) < 0.01f ) + { + m_flThrust = 0.0f; + } + else if ( flThrottle > 0.0f ) + { + m_flThrust = AIRBOAT_THRUST_MAX * flThrottle; + } + else if ( flThrottle < 0.0f ) + { + m_flThrust = AIRBOAT_THRUST_MAX_REVERSE * flThrottle; + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// stop_wheel - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::fix_wheel(IVP_POS_WHEEL pos, IVP_BOOL stop_wheel) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + wheel->wheel_is_fixed = stop_wheel; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// friction - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_friction_of_wheel( IVP_POS_WHEEL pos, IVP_FLOAT friction ) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + wheel->friction_of_wheel = friction; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// stabi_constant - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_stabilizer_constant(IVP_POS_AXIS pos, IVP_FLOAT stabi_constant) +{ + IVP_Raycast_Airboat_Axle *pAxle = get_axle( pos ); + pAxle->stabilizer_constant = stabi_constant; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : fast_turn_factor_ - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_fast_turn_factor( IVP_FLOAT fast_turn_factor_ ) +{ + //fast_turn_factor = fast_turn_factor_; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : force - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::change_body_downforce(IVP_FLOAT force) +{ + down_force = force; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Output : IVP_CONTROLLER_PRIORITY +//----------------------------------------------------------------------------- +IVP_CONTROLLER_PRIORITY CPhysics_Airboat::get_controller_priority() +{ + return IVP_CP_CONSTRAINTS_MAX; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : steering_angle_in - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::do_steering( IVP_FLOAT steering_angle_in, bool bAnalog ) +{ + // Check for a change. + if ( m_SteeringAngle == steering_angle_in) + return; + + MEM_ALLOC_CREDIT(); + + // Set the new steering angle. + m_bAnalogSteering = bAnalog; + m_SteeringAngle = steering_angle_in; + + // Make sure the simulation is awake - we just go input. + m_pAirboatBody->get_environment()->get_controller_manager()->ensure_controller_in_simulation( this ); + + // Steer each wheel. + for ( int iWheel = 0; iWheel < wheels_per_axis; ++iWheel ) + { + do_steering_wheel( IVP_POS_WHEEL( iWheel ), m_SteeringAngle ); + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : pos - +// Output : IVP_DOUBLE +//----------------------------------------------------------------------------- +IVP_DOUBLE CPhysics_Airboat::get_wheel_angular_velocity(IVP_POS_WHEEL pos) +{ + IVP_Raycast_Airboat_Wheel *wheel = get_wheel(pos); + return wheel->wheel_angular_velocity; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : index - +// Output : IVP_DOUBLE +//----------------------------------------------------------------------------- +IVP_DOUBLE CPhysics_Airboat::get_body_speed(IVP_COORDINATE_INDEX index) +{ + // return (IVP_FLOAT)car_body->get_geom_center_speed(); + IVP_U_Float_Point *vec_ws = &m_pAirboatBody->get_core()->speed; + // works well as we do not use merged cores + const IVP_U_Matrix *mat_ws = m_pAirboatBody->get_core()->get_m_world_f_core_PSI(); + IVP_U_Point orientation; + mat_ws->get_col(index, &orientation); + + return orientation.dot_product(vec_ws); +}; + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +IVP_DOUBLE CPhysics_Airboat::get_orig_front_wheel_distance() +{ + IVP_U_Float_Point *left_wheel_cs = &this->get_wheel(IVP_FRONT_LEFT)->hp_cs; + IVP_U_Float_Point *right_wheel_cs = &this->get_wheel(IVP_FRONT_RIGHT)->hp_cs; + + IVP_DOUBLE dist = left_wheel_cs->k[this->index_x] - right_wheel_cs->k[this->index_x]; + + return IVP_Inline_Math::fabsd(dist); // was fabs, which was a sml call +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +IVP_DOUBLE CPhysics_Airboat::get_orig_axles_distance() +{ + IVP_U_Float_Point *front_wheel_cs = &this->get_wheel(IVP_FRONT_LEFT)->hp_cs; + IVP_U_Float_Point *rear_wheel_cs = &this->get_wheel(IVP_REAR_LEFT)->hp_cs; + + IVP_DOUBLE dist = front_wheel_cs->k[this->index_z] - rear_wheel_cs->k[this->index_z]; + + return IVP_Inline_Math::fabsd(dist); // was fabs, which was a sml call +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : *array_of_skid_info_out - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::get_skid_info( IVP_Wheel_Skid_Info *array_of_skid_info_out) +{ + for ( int w = 0; w < n_wheels; w++) + { + IVP_Wheel_Skid_Info &info = array_of_skid_info_out[w]; + //IVP_Constraint_Car_Object *wheel = car_constraint_solver->wheel_objects.element_at(w); + info.last_contact_position_ws.set_to_zero(); // = wheel->last_contact_position_ws; + info.last_skid_value = 0.0f; // wheel->last_skid_value; + info.last_skid_time = 0.0f; //wheel->last_skid_time; + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::InitRaycastCarEnvironment( IVP_Environment *pEnvironment, + const IVP_Template_Car_System *pCarSystemTemplate ) +{ + // Copies of the car system template component indices and handedness. + index_x = pCarSystemTemplate->index_x; + index_y = pCarSystemTemplate->index_y; + index_z = pCarSystemTemplate->index_z; + is_left_handed = pCarSystemTemplate->is_left_handed; + + IVP_Standard_Gravity_Controller *pGravityController = new IVP_Standard_Gravity_Controller(); + IVP_U_Point vecGravity( 0.0f, AIRBOAT_GRAVITY, 0.0f ); + pGravityController->grav_vec.set( &vecGravity ); + + BEGIN_IVP_ALLOCATION(); + + m_pAirboatBody->get_core()->add_core_controller( pGravityController ); + + // Add this controller to the physics environment and setup the objects gravity. + pEnvironment->get_controller_manager()->announce_controller_to_environment( this ); + + END_IVP_ALLOCATION(); + + extra_gravity = pCarSystemTemplate->extra_gravity_force_value; + + // This works because gravity is still int the same direction, just smaller. + if ( pEnvironment->get_gravity()->k[index_y] > 0 ) + { + gravity_y_direction = 1.0f; + } + else + { + gravity_y_direction = -1.0f; + } + normized_gravity_ws.set( pEnvironment->get_gravity() ); + normized_gravity_ws.normize(); +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::InitRaycastCarBody( const IVP_Template_Car_System *pCarSystemTemplate ) +{ + // Car body attributes. + n_wheels = pCarSystemTemplate->n_wheels; + n_axis = pCarSystemTemplate->n_axis; + wheels_per_axis = n_wheels / n_axis; + + // Add the car body "core" to the list of raycast car controller "cores." + m_pAirboatBody = pCarSystemTemplate->car_body; + this->vector_of_cores.add( m_pAirboatBody->get_core() ); + + // Init extra downward force applied to car. + down_force_vertical_offset = pCarSystemTemplate->body_down_force_vertical_offset; + down_force = 0.0f; + + // Initialize. + for ( int iAxis = 0; iAxis < 3; ++iAxis ) + { + m_pAirboatBody->get_core()->rot_speed.k[iAxis] = 0.0f; + m_pAirboatBody->get_core()->speed.k[iAxis] = 0.0f; + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::InitRaycastCarWheels( const IVP_Template_Car_System *pCarSystemTemplate ) +{ + IVP_U_Matrix m_core_f_object; + m_pAirboatBody->calc_m_core_f_object( &m_core_f_object ); + + // Initialize the car wheel system. + for ( int iWheel = 0; iWheel < n_wheels; iWheel++ ) + { + // Get and clear out memory for the current raycast wheel. + IVP_Raycast_Airboat_Wheel *pRaycastWheel = get_wheel( IVP_POS_WHEEL( iWheel ) ); + P_MEM_CLEAR( pRaycastWheel ); + + // Put the wheel in car space. + m_core_f_object.vmult4( &pCarSystemTemplate->wheel_pos_Bos[iWheel], &pRaycastWheel->hp_cs ); + m_core_f_object.vmult4( &pCarSystemTemplate->trace_pos_Bos[iWheel], &pRaycastWheel->raycast_start_cs ); + + // Add in the raycast start offset. + pRaycastWheel->raycast_length = AIRBOAT_RAYCAST_DIST; + pRaycastWheel->raycast_dir_cs.set_to_zero(); + pRaycastWheel->raycast_dir_cs.k[index_y] = gravity_y_direction; + + // Spring (Shocks) data. + pRaycastWheel->spring_len = -pCarSystemTemplate->spring_pre_tension[iWheel]; + + pRaycastWheel->spring_direction_cs.set_to_zero(); + pRaycastWheel->spring_direction_cs.k[index_y] = gravity_y_direction; + + pRaycastWheel->spring_constant = pCarSystemTemplate->spring_constant[iWheel]; + pRaycastWheel->spring_damp_relax = pCarSystemTemplate->spring_dampening[iWheel]; + pRaycastWheel->spring_damp_compress = pCarSystemTemplate->spring_dampening_compression[iWheel]; + + // Wheel data. + pRaycastWheel->friction_of_wheel = 1.0f;//pCarSystemTemplate->friction_of_wheel[iWheel]; + pRaycastWheel->wheel_radius = pCarSystemTemplate->wheel_radius[iWheel]; + pRaycastWheel->inv_wheel_radius = 1.0f / pCarSystemTemplate->wheel_radius[iWheel]; + + do_steering_wheel( IVP_POS_WHEEL( iWheel ), 0.0f ); + + pRaycastWheel->wheel_is_fixed = IVP_FALSE; + pRaycastWheel->max_rotation_speed = pCarSystemTemplate->wheel_max_rotation_speed[iWheel>>1]; + + pRaycastWheel->wheel_is_fixed = IVP_TRUE; + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CPhysics_Airboat::InitRaycastCarAxes( const IVP_Template_Car_System *pCarSystemTemplate ) +{ + m_SteeringAngle = -1.0f; // make sure next call is not optimized + this->do_steering( 0.0f, false ); // make sure next call gets through + + for ( int iAxis = 0; iAxis < n_axis; iAxis++ ) + { + IVP_Raycast_Airboat_Axle *pAxle = get_axle( IVP_POS_AXIS( iAxis ) ); + pAxle->stabilizer_constant = pCarSystemTemplate->stabilizer_constant[iAxis]; + } +} + + +//----------------------------------------------------------------------------- +// Purpose: Debug data for use in vphysics and the engine to visualize car data. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::SetCarSystemDebugData( const IVP_CarSystemDebugData_t &carSystemDebugData ) +{ + // Wheels (raycast data only!) + for ( int iWheel = 0; iWheel < IVP_RAYCAST_AIRBOAT_MAX_WHEELS; ++iWheel ) + { + m_CarSystemDebugData.wheelRaycasts[iWheel][0] = carSystemDebugData.wheelRaycasts[iWheel][0]; + m_CarSystemDebugData.wheelRaycasts[iWheel][1] = carSystemDebugData.wheelRaycasts[iWheel][1]; + m_CarSystemDebugData.wheelRaycastImpacts[iWheel] = carSystemDebugData.wheelRaycastImpacts[iWheel]; + } +} + + +//----------------------------------------------------------------------------- +// Purpose: Debug data for use in vphysics and the engine to visualize car data. +//----------------------------------------------------------------------------- +void CPhysics_Airboat::GetCarSystemDebugData( IVP_CarSystemDebugData_t &carSystemDebugData ) +{ + // Wheels (raycast data only!) + for ( int iWheel = 0; iWheel < IVP_RAYCAST_AIRBOAT_MAX_WHEELS; ++iWheel ) + { + carSystemDebugData.wheelRaycasts[iWheel][0] = m_CarSystemDebugData.wheelRaycasts[iWheel][0]; + carSystemDebugData.wheelRaycasts[iWheel][1] = m_CarSystemDebugData.wheelRaycasts[iWheel][1]; + carSystemDebugData.wheelRaycastImpacts[iWheel] = m_CarSystemDebugData.wheelRaycastImpacts[iWheel]; + } +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Output : IVP_U_Vector<IVP_Core> +//----------------------------------------------------------------------------- +IVP_U_Vector<IVP_Core> *CPhysics_Airboat::get_associated_controlled_cores( void ) +{ + return &vector_of_cores; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : *core - +//----------------------------------------------------------------------------- +void CPhysics_Airboat::core_is_going_to_be_deleted_event( IVP_Core *core ) +{ + P_DELETE_THIS(this); +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : i - +// Output : IVP_Raycast_Airboat_Axle +//----------------------------------------------------------------------------- +IVP_Raycast_Airboat_Axle *CPhysics_Airboat::get_axle( IVP_POS_AXIS i ) +{ + return &m_aAirboatAxles[i]; +} + + +//----------------------------------------------------------------------------- +// Purpose: +// Input : i - +// Output : IVP_Raycast_Airboat_Wheel +//----------------------------------------------------------------------------- +IVP_Raycast_Airboat_Wheel *CPhysics_Airboat::get_wheel( IVP_POS_WHEEL i ) +{ + return &m_aAirboatWheels[i]; +} + + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +IVP_Controller_Raycast_Airboat_Vector_of_Cores_1::IVP_Controller_Raycast_Airboat_Vector_of_Cores_1(): + IVP_U_Vector<IVP_Core>( &elem_buffer[0],1 ) +{ +} + diff --git a/vphysics/physics_airboat.h b/vphysics/physics_airboat.h new file mode 100644 index 0000000..c1f649c --- /dev/null +++ b/vphysics/physics_airboat.h @@ -0,0 +1,303 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//=============================================================================// + +#ifndef PHYSICS_AIRBOAT_H +#define PHYSICS_AIRBOAT_H +#ifdef _WIN32 +#pragma once +#endif + +#include "ivp_controller.hxx" +#include "ivp_car_system.hxx" + + +class IPhysicsObject; +class IVP_Ray_Solver_Template; +class IVP_Ray_Hit; +class IVP_Event_Sim; + + +#define IVP_RAYCAST_AIRBOAT_MAX_WHEELS 4 + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +class IVP_Raycast_Airboat_Wheel +{ +public: + + // static section + IVP_U_Float_Point hp_cs; // hard point core system projected on y plane + IVP_U_Float_Point raycast_start_cs; // ray cast start position + IVP_U_Float_Point raycast_dir_cs; + IVP_FLOAT raycast_length; + + IVP_U_Float_Point spring_direction_cs; // spring direction in core-space + IVP_FLOAT distance_orig_hp_to_hp; // distance hp is moved by projecting it onto the y - plane + IVP_FLOAT spring_len; // == pretension + distance_orig_hp_to_hp + IVP_FLOAT spring_constant; // shock at wheel spring constant + IVP_FLOAT spring_damp_relax; // shock at wheel spring dampening during relaxation + IVP_FLOAT spring_damp_compress; // shock at wheel spring dampening during compression + + IVP_FLOAT max_rotation_speed; // max rotational speed of the wheel + + IVP_FLOAT wheel_radius; // wheel radius + IVP_FLOAT inv_wheel_radius; // inverse wheel radius + IVP_FLOAT friction_of_wheel; // wheel friction + + // dynamic section + IVP_FLOAT torque; // torque applied to wheel + IVP_BOOL wheel_is_fixed; // eg. handbrake (fixed = stationary) + IVP_U_Float_Point axis_direction_cs; // axle direction in core-space + IVP_FLOAT angle_wheel; // wheel angle + IVP_FLOAT wheel_angular_velocity; // angular velocity of wheel + + // out + IVP_U_Float_Point surface_speed_of_wheel_on_ground_ws; // actual speed in world-space + IVP_FLOAT pressure; // force from gravity, mass of car, stabilizers, etc. on wheel + IVP_FLOAT raycast_dist; // raycast distance to impact for wheel +}; + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +class IVP_Raycast_Airboat_Impact +{ +public: + + IVP_FLOAT friction_value; // combined (multiply) frictional value of impact surface and wheel + IVP_FLOAT stabilizer_force; // force on wheel due to axle stabilization + IVP_Real_Object *moveable_object_hit_by_ray; // moveable physics object hit by raycast + + IVP_U_Float_Point raycast_dir_ws; // raycast direction in world-space + IVP_U_Float_Point spring_direction_ws; // spring direction (raycast for impact direction) in world-space + IVP_U_Float_Point surface_speed_wheel_ws; // wheel speed in world-space + IVP_U_Float_Point projected_surface_speed_wheel_ws; // ??? + IVP_U_Float_Point axis_direction_ws; // axle direction in world-space + IVP_U_Float_Point projected_axis_direction_ws; // ??? + + IVP_FLOAT forces_needed_to_drive_straight; // forces need to keep the vehicle driving straight (attempt and directional wheel friction) + IVP_FLOAT inv_normal_dot_dir; // ??? + + // Impact information. + IVP_BOOL bImpact; // Had an impact? + IVP_BOOL bImpactWater; // Impact with water? + IVP_BOOL bInWater; // Point in water? + IVP_U_Point vecImpactPointWS; // Impact point in world-space. + IVP_U_Float_Point vecImpactNormalWS; // Impact normal in world-space. + IVP_FLOAT flDepth; // Distance to water surface. + IVP_FLOAT flFriction; // Friction at impact point. + IVP_FLOAT flDampening; // Dampening at surface. + int nSurfaceProps; // Surface property! +}; + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +class IVP_Raycast_Airboat_Axle +{ +public: + + IVP_FLOAT stabilizer_constant; // axle (for wheels) stabilizer constant +}; + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +class IVP_Controller_Raycast_Airboat_Vector_of_Cores_1: public IVP_U_Vector<IVP_Core> +{ + void *elem_buffer[1]; + +public: + + IVP_Controller_Raycast_Airboat_Vector_of_Cores_1(); +}; + + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +class CPhysics_Airboat : public IVP_Car_System, protected IVP_Controller_Dependent +{ + +public: + + CPhysics_Airboat( IVP_Environment *env, const IVP_Template_Car_System *t, IPhysicsGameTrace *pGameTrace ); + virtual ~CPhysics_Airboat(); + + void update_wheel_positions( void ) {} + void SetWheelFriction( int iWheel, float flFriction ); + + IPhysicsObject *GetWheel( int index ); + + virtual const char *get_controller_name() { return "sys:airboat"; } + +protected: + + void InitAirboat( const IVP_Template_Car_System *pCarSystem ); + float GetWaterDepth( Ray_t *pGameRay, IPhysicsObject *pPhysAirboat ); + + // Purpose: Deconstructor + void PerformFrictionNotification( float flEliminatedEnergy, float dt, int nSurfaceProp, IPhysicsCollisionData *pCollisionData ); + + void do_raycasts_gameside( int nRaycastCount, IVP_Ray_Solver_Template *pRays, IVP_Raycast_Airboat_Impact *pImpacts ); + void pre_raycasts_gameside( int nRaycastCount, IVP_Ray_Solver_Template *pRays, Ray_t *pGameRays, IVP_Raycast_Airboat_Impact *pImpacts ); + + IVP_Real_Object *m_pWheels[IVP_RAYCAST_AIRBOAT_MAX_WHEELS]; + IPhysicsGameTrace *m_pGameTrace; + +public: + + // Steering + void do_steering_wheel(IVP_POS_WHEEL wheel_pos, IVP_FLOAT s_angle); // called by do_steering() + + // Car Adjustment + void change_spring_constant(IVP_POS_WHEEL pos, IVP_FLOAT spring_constant); // [Newton/meter] + void change_spring_dampening(IVP_POS_WHEEL pos, IVP_FLOAT spring_dampening); // when spring is relaxing spring + void change_spring_dampening_compression(IVP_POS_WHEEL pos, IVP_FLOAT spring_dampening); // [Newton/meter] for compressing spring + void change_max_body_force(IVP_POS_WHEEL , IVP_FLOAT mforce) {} + void change_spring_pre_tension(IVP_POS_WHEEL pos, IVP_FLOAT pre_tension_length); + void change_spring_length(IVP_POS_WHEEL pos, IVP_FLOAT spring_length); + + void change_stabilizer_constant(IVP_POS_AXIS pos, IVP_FLOAT stabi_constant); // [Newton/meter] + void change_fast_turn_factor( IVP_FLOAT fast_turn_factor_ ); // not implemented for raycasts + void change_wheel_torque(IVP_POS_WHEEL pos, IVP_FLOAT torque); + IVP_FLOAT get_wheel_torque(IVP_POS_WHEEL wheel_nr); + + void update_throttle( IVP_FLOAT flThrottle ); + + void update_body_countertorque() {} + + void change_body_downforce(IVP_FLOAT force); // extra force to keep flipped objects flipped over + + void fix_wheel( IVP_POS_WHEEL, IVP_BOOL stop_wheel ); // stop wheel completely (e.g. handbrake ) + void change_friction_of_wheel( IVP_POS_WHEEL pos, IVP_FLOAT friction ); + void set_powerslide( float frontAccel, float rearAccel ) {} + + // Car Info + IVP_DOUBLE get_body_speed(IVP_COORDINATE_INDEX idx_z = IVP_INDEX_Z); // km/h in 'z' direction + IVP_DOUBLE get_wheel_angular_velocity(IVP_POS_WHEEL); + IVP_DOUBLE get_orig_front_wheel_distance(); + IVP_DOUBLE get_orig_axles_distance(); + void get_skid_info( IVP_Wheel_Skid_Info *array_of_skid_info_out); + + void get_wheel_position(IVP_U_Point *position_ws_out, IVP_U_Quat *direction_ws_out); + + // Methods: 2nd Level, based on primitives + virtual void do_steering(IVP_FLOAT steering_angle_in, bool bAnalog); // default implementation updates this->steering_angle + + // + // Booster (the airboat has no booster). + // + virtual bool IsBoosting(void) { return false; } + virtual void set_booster_acceleration( IVP_FLOAT acceleration) {} + virtual void activate_booster(IVP_FLOAT thrust, IVP_FLOAT duration, IVP_FLOAT delay) {} + virtual void update_booster(IVP_FLOAT delta_time) {} + virtual IVP_FLOAT get_booster_delay() { return 0; } + virtual IVP_FLOAT get_booster_time_to_go() { return 0; } + + // Debug + void SetCarSystemDebugData( const IVP_CarSystemDebugData_t &carSystemDebugData ); + void GetCarSystemDebugData( IVP_CarSystemDebugData_t &carSystemDebugData ); + +protected: + + IVP_Core *m_pCore; + IVP_U_Float_Point m_vecLocalVelocity; + float m_flSpeed; + IVP_Real_Object *m_pAirboatBody; // *car_body + + // Wheels/Axles. + short n_wheels; + short n_axis; + short wheels_per_axis; + IVP_Raycast_Airboat_Wheel m_aAirboatWheels[IVP_RAYCAST_AIRBOAT_MAX_WHEELS]; // wheel_of_car + IVP_Raycast_Airboat_Axle m_aAirboatAxles[IVP_RAYCAST_AIRBOAT_MAX_WHEELS/2]; // axis_of_car + + // Gravity. + IVP_FLOAT gravity_y_direction; // +/-1 + IVP_U_Float_Point normized_gravity_ws; + IVP_FLOAT extra_gravity; + + // Orientation. + IVP_COORDINATE_INDEX index_x; + IVP_COORDINATE_INDEX index_y; + IVP_COORDINATE_INDEX index_z; + IVP_BOOL is_left_handed; + + // Speed. + IVP_FLOAT max_speed; + + // + IVP_FLOAT down_force; + IVP_FLOAT down_force_vertical_offset; + + // Steering + IVP_FLOAT m_SteeringAngle; + bool m_bSteeringReversed; + bool m_bAnalogSteering; + IVP_FLOAT m_flPrevSteeringAngle; + IVP_FLOAT m_flSteerTime; // Number of seconds we've steered in this direction. + + // Thrust. + IVP_FLOAT m_flThrust; + + bool m_bAirborne; // Whether we are airborne or not. + IVP_FLOAT m_flAirTime; // How long we've been airborne (if we are). + bool m_bWeakJump; // Set when we become airborne while going slow. + + // Pitch and roll stabilizers. + IVP_FLOAT m_flPitchErrorPrev; + IVP_FLOAT m_flRollErrorPrev; + + // Debugging! + IVP_CarSystemDebugData_t m_CarSystemDebugData; + +protected: + + IVP_Raycast_Airboat_Wheel *get_wheel( IVP_POS_WHEEL i ); + IVP_Raycast_Airboat_Axle *get_axle( IVP_POS_AXIS i ); + + virtual void core_is_going_to_be_deleted_event( IVP_Core * ); + virtual IVP_U_Vector<IVP_Core> *get_associated_controlled_cores( void ); + + virtual void do_simulation_controller(IVP_Event_Sim *,IVP_U_Vector<IVP_Core> *core_list); + virtual IVP_CONTROLLER_PRIORITY get_controller_priority(); + +private: + + // Initialization. + void InitRaycastCarEnvironment( IVP_Environment *pEnvironment, const IVP_Template_Car_System *pCarSystemTemplate ); + void InitRaycastCarBody( const IVP_Template_Car_System *pCarSystemTemplate ); + void InitRaycastCarWheels( const IVP_Template_Car_System *pCarSystemTemplate ); + void InitRaycastCarAxes( const IVP_Template_Car_System *pCarSystemTemplate ); + + // Raycasts for simulation. + void PreRaycasts( IVP_Ray_Solver_Template *pRaySolverTemplates, const IVP_U_Matrix *m_world_f_core, IVP_Raycast_Airboat_Impact *pImpacts ); + bool PostRaycasts( IVP_Ray_Solver_Template *pRaySolverTemplates, const IVP_U_Matrix *matWorldFromCore, IVP_Raycast_Airboat_Impact *pImpacts ); + + // Simulation. + void DoSimulationPontoons( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ); + void DoSimulationPontoonsGround( IVP_Raycast_Airboat_Wheel *pPontoonPoint, IVP_Raycast_Airboat_Impact *pImpact, IVP_Event_Sim *pEventSim ); + void DoSimulationPontoonsWater( IVP_Raycast_Airboat_Wheel *pPontoonPoint, IVP_Raycast_Airboat_Impact *pImpact, IVP_Event_Sim *pEventSim ); + void DoSimulationDrag( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ); + void DoSimulationTurbine( IVP_Event_Sim *pEventSim ); + void DoSimulationSteering( IVP_Event_Sim *pEventSim ); + void DoSimulationKeepUprightPitch( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ); + void DoSimulationKeepUprightRoll( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ); + void DoSimulationGravity( IVP_Event_Sim *pEventSim ); + + int CountSurfaceContactPoints( IVP_Raycast_Airboat_Impact *pImpacts ); + void UpdateAirborneState( IVP_Raycast_Airboat_Impact *pImpacts, IVP_Event_Sim *pEventSim ); + + float ComputeFrontPontoonWaveNoise( int nPontoonIndex, float flSpeedRatio ); + + void CalcImpactPosition( IVP_Ray_Solver_Template *pRaySolver, IVP_Raycast_Airboat_Wheel *pPontoonPoint, + IVP_Raycast_Airboat_Impact *pImpacts ); + + IVP_Controller_Raycast_Airboat_Vector_of_Cores_1 vector_of_cores; +}; + +#endif // PHYSICS_AIRBOAT_H diff --git a/vphysics/physics_collide.cpp b/vphysics/physics_collide.cpp new file mode 100644 index 0000000..6a6d185 --- /dev/null +++ b/vphysics/physics_collide.cpp @@ -0,0 +1,1932 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" + +#include "ivp_surbuild_pointsoup.hxx" +#include "ivp_surbuild_ledge_soup.hxx" +#include "ivp_surman_polygon.hxx" +#include "ivp_compact_surface.hxx" +#include "ivp_compact_ledge.hxx" +#include "ivp_compact_ledge_solver.hxx" +#include "ivp_halfspacesoup.hxx" +#include "ivp_surbuild_halfspacesoup.hxx" +#include "ivp_template_surbuild.hxx" +#include "hk_mopp/ivp_surbuild_mopp.hxx" +#include "hk_mopp/ivp_surman_mopp.hxx" +#include "hk_mopp/ivp_compact_mopp.hxx" +#include "ivp_surbuild_polygon_convex.hxx" +#include "ivp_templates_intern.hxx" + +#include "cmodel.h" +#include "physics_trace.h" +#include "vcollide_parse_private.h" +#include "physics_virtualmesh.h" + +#include "mathlib/polyhedron.h" +#include "tier1/byteswap.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +class CPhysCollideCompactSurface; +struct bboxcache_t +{ + Vector mins; + Vector maxs; + CPhysCollideCompactSurface *pCollide; +}; + +class CPhysicsCollision : public IPhysicsCollision +{ +public: + CPhysicsCollision() + { + } + CPhysConvex *ConvexFromVerts( Vector **pVerts, int vertCount ); + CPhysConvex *ConvexFromVertsFast( Vector **pVerts, int vertCount ); + CPhysConvex *ConvexFromPlanes( float *pPlanes, int planeCount, float mergeDistance ); + CPhysConvex *ConvexFromConvexPolyhedron( const CPolyhedron &ConvexPolyhedron ); + void ConvexesFromConvexPolygon( const Vector &vPolyNormal, const Vector *pPoints, int iPointCount, CPhysConvex **pOutput ); + CPhysConvex *RebuildConvexFromPlanes( CPhysConvex *pConvex, float mergeDistance ); + float ConvexVolume( CPhysConvex *pConvex ); + float ConvexSurfaceArea( CPhysConvex *pConvex ); + CPhysCollide *ConvertConvexToCollide( CPhysConvex **pConvex, int convexCount ); + CPhysCollide *ConvertConvexToCollideParams( CPhysConvex **pConvex, int convexCount, const convertconvexparams_t &convertParams ); + + + CPolyhedron *PolyhedronFromConvex( CPhysConvex * const pConvex, bool bUseTempPolyhedron ); + int GetConvexesUsedInCollideable( const CPhysCollide *pCollideable, CPhysConvex **pOutputArray, int iOutputArrayLimit ); + + // store game-specific data in a convex solid + void SetConvexGameData( CPhysConvex *pConvex, unsigned int gameData ); + void ConvexFree( CPhysConvex *pConvex ); + + CPhysPolysoup *PolysoupCreate( void ); + void PolysoupDestroy( CPhysPolysoup *pSoup ); + void PolysoupAddTriangle( CPhysPolysoup *pSoup, const Vector &a, const Vector &b, const Vector &c, int materialIndex7bits ); + CPhysCollide *ConvertPolysoupToCollide( CPhysPolysoup *pSoup, bool useMOPP = true ); + + int CollideSize( CPhysCollide *pCollide ); + int CollideWrite( char *pDest, CPhysCollide *pCollide, bool bSwap = false ); + // Get the AABB of an oriented collide + virtual void CollideGetAABB( Vector *pMins, Vector *pMaxs, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles ); + virtual Vector CollideGetExtent( const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, const Vector &direction ); + // compute the volume of a collide + virtual float CollideVolume( CPhysCollide *pCollide ); + virtual float CollideSurfaceArea( CPhysCollide *pCollide ); + + // Free a collide that was created with ConvertConvexToCollide() + // UNDONE: Move this up near the other Collide routines when the version is changed + virtual void DestroyCollide( CPhysCollide *pCollide ); + + CPhysCollide *BBoxToCollide( const Vector &mins, const Vector &maxs ); + CPhysConvex *BBoxToConvex( const Vector &mins, const Vector &maxs ); + + // loads a set of solids into a vcollide_t + virtual void VCollideLoad( vcollide_t *pOutput, int solidCount, const char *pBuffer, int size, bool swap ); + // destroyts the set of solids created by VCollideLoad + virtual void VCollideUnload( vcollide_t *pVCollide ); + + // Trace an AABB against a collide + void TraceBox( const Vector &start, const Vector &end, const Vector &mins, const Vector &maxs, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, trace_t *ptr ); + void TraceBox( const Ray_t &ray, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, trace_t *ptr ); + void TraceBox( const Ray_t &ray, unsigned int contentsMask, IConvexInfo *pConvexInfo, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, trace_t *ptr ); + // Trace one collide against another + void TraceCollide( const Vector &start, const Vector &end, const CPhysCollide *pSweepCollide, const QAngle &sweepAngles, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, trace_t *ptr ); + bool IsBoxIntersectingCone( const Vector &boxAbsMins, const Vector &boxAbsMaxs, const truncatedcone_t &cone ); + + // begins parsing a vcollide. NOTE: This keeps pointers to the text + // If you delete the text and call members of IVPhysicsKeyParser, it will crash + virtual IVPhysicsKeyParser *VPhysicsKeyParserCreate( const char *pKeyData ); + // Free the parser created by VPhysicsKeyParserCreate + virtual void VPhysicsKeyParserDestroy( IVPhysicsKeyParser *pParser ); + + // creates a list of verts from a collision mesh + int CreateDebugMesh( const CPhysCollide *pCollisionModel, Vector **outVerts ); + // destroy the list of verts created by CreateDebugMesh + void DestroyDebugMesh( int vertCount, Vector *outVerts ); + // create a queryable version of the collision model + ICollisionQuery *CreateQueryModel( CPhysCollide *pCollide ); + // destroy the queryable version + void DestroyQueryModel( ICollisionQuery *pQuery ); + + virtual IPhysicsCollision *ThreadContextCreate( void ); + virtual void ThreadContextDestroy( IPhysicsCollision *pThreadContex ); + virtual unsigned int ReadStat( int statID ) { return 0; } + virtual void CollideGetMassCenter( CPhysCollide *pCollide, Vector *pOutMassCenter ); + virtual void CollideSetMassCenter( CPhysCollide *pCollide, const Vector &massCenter ); + + virtual int CollideIndex( const CPhysCollide *pCollide ); + virtual Vector CollideGetOrthographicAreas( const CPhysCollide *pCollide ); + virtual void OutputDebugInfo( const CPhysCollide *pCollide ); + virtual CPhysCollide *CreateVirtualMesh(const virtualmeshparams_t ¶ms) { return ::CreateVirtualMesh(params); } + virtual bool GetBBoxCacheSize( int *pCachedSize, int *pCachedCount ); + + virtual bool SupportsVirtualMesh() { return true; } + + virtual CPhysCollide *UnserializeCollide( char *pBuffer, int size, int index ); + virtual void CollideSetOrthographicAreas( CPhysCollide *pCollide, const Vector &areas ); + +private: + void InitBBoxCache(); + bool IsBBoxCache( CPhysCollide *pCollide ); + void AddBBoxCache( CPhysCollideCompactSurface *pCollide, const Vector &mins, const Vector &maxs ); + CPhysCollideCompactSurface *GetBBoxCache( const Vector &mins, const Vector &maxs ); + CPhysCollideCompactSurface *FastBboxCollide( const CPhysCollideCompactSurface *pCollide, const Vector &mins, const Vector &maxs ); + +private: + CPhysicsTrace m_traceapi; + CUtlVector<bboxcache_t> m_bboxCache; + byte m_bboxVertMap[8]; +}; + +CPhysicsCollision g_PhysicsCollision; +IPhysicsCollision *physcollision = &g_PhysicsCollision; +EXPOSE_SINGLE_INTERFACE_GLOBALVAR( CPhysicsCollision, IPhysicsCollision, VPHYSICS_COLLISION_INTERFACE_VERSION, g_PhysicsCollision ); + + +//----------------------------------------------------------------------------- +// Abstract compact_surface vs. compact_mopp +//----------------------------------------------------------------------------- +#define IVP_COMPACT_SURFACE_ID MAKEID('I','V','P','S') +#define IVP_COMPACT_SURFACE_ID_SWAPPED MAKEID('S','P','V','I') +#define IVP_COMPACT_MOPP_ID MAKEID('M','O','P','P') +#define VPHYSICS_COLLISION_ID MAKEID('V','P','H','Y') +#define VPHYSICS_COLLISION_VERSION 0x0100 +// You can disable all of the havok Mopp collision model building by undefining this symbol +#define ENABLE_IVP_MOPP 0 + +struct physcollideheader_t +{ + DECLARE_BYTESWAP_DATADESC(); + int vphysicsID; + short version; + short modelType; + + void Defaults( short inputModelType ) + { + vphysicsID = VPHYSICS_COLLISION_ID; + + version = VPHYSICS_COLLISION_VERSION; + modelType = inputModelType; + } +}; + +struct compactsurfaceheader_t : public physcollideheader_t +{ + DECLARE_BYTESWAP_DATADESC(); + int surfaceSize; + Vector dragAxisAreas; + int axisMapSize; + + void CompactSurface( const IVP_Compact_Surface *pSurface, const Vector &orthoAreas ) + { + Defaults( COLLIDE_POLY ); + surfaceSize = pSurface->byte_size; + dragAxisAreas = orthoAreas; + axisMapSize = 0; // NOTE: not yet supported + } +}; + +BEGIN_BYTESWAP_DATADESC( physcollideheader_t ) + DEFINE_FIELD( vphysicsID, FIELD_INTEGER ), + DEFINE_FIELD( version, FIELD_SHORT), + DEFINE_FIELD( modelType, FIELD_SHORT ), +END_BYTESWAP_DATADESC() + +BEGIN_BYTESWAP_DATADESC_( compactsurfaceheader_t, physcollideheader_t ) + DEFINE_FIELD( surfaceSize, FIELD_INTEGER ), + DEFINE_FIELD( dragAxisAreas, FIELD_VECTOR ), + DEFINE_FIELD( axisMapSize, FIELD_INTEGER ), +END_BYTESWAP_DATADESC() + +#if ENABLE_IVP_MOPP +struct moppheader_t : public physcollideheader_t +{ + int moppSize; + void Mopp( const IVP_Compact_Mopp *pMopp ) + { + Defaults( COLLIDE_MOPP ); + moppSize = pMopp->byte_size; + } +}; +#endif + +#if ENABLE_IVP_MOPP +class CPhysCollideMopp : public CPhysCollide +{ +public: + CPhysCollideMopp( const moppheader_t *pHeader ); + CPhysCollideMopp( IVP_Compact_Mopp *pMopp ); + CPhysCollideMopp( const char *pBuffer, unsigned int size ); + ~CPhysCollideMopp(); + + void Init( const char *pBuffer, unsigned int size ); + + // IPhysCollide + virtual int GetVCollideIndex() const { return 0; } + virtual IVP_SurfaceManager *CreateSurfaceManager( short & ) const; + virtual void GetAllLedges( IVP_U_BigVector<IVP_Compact_Ledge> &ledges ) const; + virtual unsigned int GetSerializationSize() const; + virtual Vector GetMassCenter() const; + virtual void SetMassCenter( const Vector &massCenter ); + virtual unsigned int SerializeToBuffer( char *pDest, bool bSwap = false ) const; + virtual void OutputDebugInfo() const; + +private: + IVP_Compact_Mopp *m_pMopp; +}; +#endif + +class CPhysCollideCompactSurface : public CPhysCollide +{ +public: + ~CPhysCollideCompactSurface(); + CPhysCollideCompactSurface( const char *pBuffer, unsigned int size, int index, bool swap = false ); + CPhysCollideCompactSurface( const compactsurfaceheader_t *pHeader, int index, bool swap = false ); + CPhysCollideCompactSurface( IVP_Compact_Surface *pSurface ); + + void Init( const char *pBuffer, unsigned int size, int index, bool swap = false ); + + // IPhysCollide + virtual int GetVCollideIndex() const { return m_pCompactSurface->dummy[0]; } + virtual IVP_SurfaceManager *CreateSurfaceManager( short & ) const; + virtual void GetAllLedges( IVP_U_BigVector<IVP_Compact_Ledge> &ledges ) const; + virtual unsigned int GetSerializationSize() const; + virtual Vector GetMassCenter() const; + virtual void SetMassCenter( const Vector &massCenter ); + virtual unsigned int SerializeToBuffer( char *pDest, bool bSwap = false ) const; + virtual Vector GetOrthographicAreas() const; + void SetOrthographicAreas( const Vector &areas ); + virtual void ComputeOrthographicAreas( float epsilon ); + virtual void OutputDebugInfo() const; + + const IVP_Compact_Surface *GetCompactSurface() const { return m_pCompactSurface; } + virtual const collidemap_t *GetCollideMap() const { return m_pCollideMap; } + +private: + + struct hullinfo_t + { + hullinfo_t() + { + hasOuterHull = false; + convexCount = 0; + } + bool hasOuterHull; + int convexCount; + }; + + void ComputeHullInfo_r( hullinfo_t *pOut, const IVP_Compact_Ledgetree_Node *node ) const; + void InitCollideMap(); + + IVP_Compact_Surface *m_pCompactSurface; + Vector m_orthoAreas; + collidemap_t *m_pCollideMap; +}; + + +static const IVP_Compact_Surface *ConvertPhysCollideToCompactSurface( const CPhysCollide *pCollide ) +{ + return pCollide->GetCompactSurface(); +} + +IVP_SurfaceManager *CreateSurfaceManager( const CPhysCollide *pCollisionModel, short &collideType ) +{ + return pCollisionModel ? pCollisionModel->CreateSurfaceManager( collideType ) : NULL; +} + +void OutputCollideDebugInfo( const CPhysCollide *pCollisionModel ) +{ + pCollisionModel->OutputDebugInfo(); +} + + +CPhysCollide *CPhysCollide::UnserializeFromBuffer( const char *pBuffer, unsigned int size, int index, bool swap ) +{ + const physcollideheader_t *pHeader = reinterpret_cast<const physcollideheader_t *>(pBuffer); + if ( pHeader->vphysicsID == VPHYSICS_COLLISION_ID ) + { + Assert(pHeader->version == VPHYSICS_COLLISION_VERSION); + switch( pHeader->modelType ) + { + case COLLIDE_POLY: + return new CPhysCollideCompactSurface( (compactsurfaceheader_t *)pHeader, index, swap ); + case COLLIDE_MOPP: +#if ENABLE_IVP_MOPP + return new CPhysCollideMopp( (moppheader_t *)pHeader ); +#else + DevMsg( 2, "Null physics model\n"); + return NULL; +#endif + default: + Assert(0); + return NULL; + } + } + const IVP_Compact_Surface *pSurface = reinterpret_cast<const IVP_Compact_Surface *>(pBuffer); + if ( pSurface->dummy[2] == IVP_COMPACT_MOPP_ID ) + { +#if ENABLE_IVP_MOPP + return new CPhysCollideMopp( pBuffer, size ); +#else + Assert(0); + return NULL; +#endif + } + if ( pSurface->dummy[2] == IVP_COMPACT_SURFACE_ID || + pSurface->dummy[2] == IVP_COMPACT_SURFACE_ID_SWAPPED || + pSurface->dummy[2] == 0 ) + { + if ( pSurface->dummy[2] == 0 ) + { + // UNDONE: Print a name here? + DevMsg( 1, "Old format .PHY file loaded!!!\n" ); + } + return new CPhysCollideCompactSurface( pBuffer, size, index, swap ); + } + + Assert(0); + return NULL; +} + +#if ENABLE_IVP_MOPP + +void CPhysCollideMopp::Init( const char *pBuffer, unsigned int size ) +{ + m_pMopp = (IVP_Compact_Mopp *)ivp_malloc_aligned( size, 32 ); + memcpy( m_pMopp, pBuffer, size ); +} + +CPhysCollideMopp::CPhysCollideMopp( const char *pBuffer, unsigned int size ) +{ + Init( pBuffer, size ); +} + +CPhysCollideMopp::CPhysCollideMopp( const moppheader_t *pHeader ) +{ + Init( (const char *)(pHeader+1), pHeader->moppSize ); +} + +CPhysCollideMopp::CPhysCollideMopp( IVP_Compact_Mopp *pMopp ) +{ + m_pMopp = pMopp; + pMopp->dummy = IVP_COMPACT_MOPP_ID; +} + +CPhysCollideMopp::~CPhysCollideMopp() +{ + ivp_free_aligned(m_pMopp); +} + +void CPhysCollideMopp::GetAllLedges( IVP_U_BigVector<IVP_Compact_Ledge> &ledges ) const +{ + IVP_Compact_Ledge_Solver::get_all_ledges( m_pMopp, &ledges ); +} + +IVP_SurfaceManager *CPhysCollideMopp::CreateSurfaceManager( short &collideType ) const +{ + collideType = COLLIDE_MOPP; + return new IVP_SurfaceManager_Mopp( m_pMopp ); +} + +unsigned int CPhysCollideMopp::GetSerializationSize() const +{ + return m_pMopp->byte_size + sizeof(moppheader_t); +} + +unsigned int CPhysCollideMopp::SerializeToBuffer( char *pDest, bool bSwap ) const +{ + moppheader_t header; + header.Mopp( m_pMopp ); + memcpy( pDest, &header, sizeof(header) ); + pDest += sizeof(header); + memcpy( pDest, m_pMopp, m_pMopp->byte_size ); + return GetSerializationSize(); +} + +Vector CPhysCollideMopp::GetMassCenter() const +{ + Vector massCenterHL; + ConvertPositionToHL( m_pMopp->mass_center, massCenterHL ); + return massCenterHL; +} + +void CPhysCollideMopp::SetMassCenter( const Vector &massCenterHL ) +{ + ConvertPositionToIVP( massCenterHL, m_pMopp->mass_center ); +} + +void CPhysCollideMopp::OutputDebugInfo() const +{ + Msg("CollisionModel: MOPP\n"); +} +#endif + +void CPhysCollideCompactSurface::InitCollideMap() +{ + m_pCollideMap = NULL; + if ( m_pCompactSurface ) + { + IVP_U_BigVector<IVP_Compact_Ledge> ledges; + GetAllLedges( ledges ); + // don't make these for really large models because there's a linear search involved in using this atm. + if ( !ledges.len() || ledges.len() > 32 ) + return; + int allocSize = sizeof(collidemap_t) + ((ledges.len()-1) * sizeof(leafmap_t)); + m_pCollideMap = (collidemap_t *)malloc(allocSize); + m_pCollideMap->leafCount = ledges.len(); + for ( int i = 0; i < ledges.len(); i++ ) + { + InitLeafmap( ledges.element_at(i), &m_pCollideMap->leafmap[i] ); + } + } +} + +void CPhysCollideCompactSurface::Init( const char *pBuffer, unsigned int size, int index, bool bSwap ) +{ + m_pCompactSurface = (IVP_Compact_Surface *)ivp_malloc_aligned( size, 32 ); + memcpy( m_pCompactSurface, pBuffer, size ); + if ( bSwap ) + { + m_pCompactSurface->byte_swap_all(); + } + m_pCompactSurface->dummy[0] = index; + m_orthoAreas.Init(1,1,1); + InitCollideMap(); +} + +CPhysCollideCompactSurface::CPhysCollideCompactSurface( const char *pBuffer, unsigned int size, int index, bool swap ) +{ + Init( pBuffer, size, index, swap ); +} +CPhysCollideCompactSurface::CPhysCollideCompactSurface( const compactsurfaceheader_t *pHeader, int index, bool swap ) +{ + Init( (const char *)(pHeader+1), pHeader->surfaceSize, index, swap ); + m_orthoAreas = pHeader->dragAxisAreas; +} + +CPhysCollideCompactSurface::CPhysCollideCompactSurface( IVP_Compact_Surface *pSurface ) +{ + m_pCompactSurface = pSurface; + pSurface->dummy[2] = IVP_COMPACT_SURFACE_ID; + m_pCompactSurface->dummy[0] = 0; + m_orthoAreas.Init(1,1,1); + InitCollideMap(); +} + +CPhysCollideCompactSurface::~CPhysCollideCompactSurface() +{ + ivp_free_aligned(m_pCompactSurface); + if ( m_pCollideMap ) + { + free(m_pCollideMap); + } +} + +IVP_SurfaceManager *CPhysCollideCompactSurface::CreateSurfaceManager( short &collideType ) const +{ + collideType = COLLIDE_POLY; + return new IVP_SurfaceManager_Polygon( m_pCompactSurface ); +} + +void CPhysCollideCompactSurface::GetAllLedges( IVP_U_BigVector<IVP_Compact_Ledge> &ledges ) const +{ + IVP_Compact_Ledge_Solver::get_all_ledges( m_pCompactSurface, &ledges ); +} + +unsigned int CPhysCollideCompactSurface::GetSerializationSize() const +{ + return m_pCompactSurface->byte_size + sizeof(compactsurfaceheader_t); +} + +unsigned int CPhysCollideCompactSurface::SerializeToBuffer( char *pDest, bool bSwap ) const +{ + compactsurfaceheader_t header; + header.CompactSurface( m_pCompactSurface, m_orthoAreas ); + if ( bSwap ) + { + CByteswap swap; + swap.ActivateByteSwapping( true ); + swap.SwapFieldsToTargetEndian( &header ); + } + memcpy( pDest, &header, sizeof(header) ); + pDest += sizeof(header); + int surfaceSize = m_pCompactSurface->byte_size; + int serializationSize = GetSerializationSize(); + if ( bSwap ) + { + m_pCompactSurface->byte_swap_all(); + } + memcpy( pDest, m_pCompactSurface, surfaceSize ); + return serializationSize; +} + +Vector CPhysCollideCompactSurface::GetMassCenter() const +{ + Vector massCenterHL; + ConvertPositionToHL( m_pCompactSurface->mass_center, massCenterHL ); + return massCenterHL; +} + +void CPhysCollideCompactSurface::SetMassCenter( const Vector &massCenterHL ) +{ + ConvertPositionToIVP( massCenterHL, m_pCompactSurface->mass_center ); +} + +Vector CPhysCollideCompactSurface::GetOrthographicAreas() const +{ + return m_orthoAreas; +} + +void CPhysCollideCompactSurface::SetOrthographicAreas( const Vector &areas ) +{ + m_orthoAreas = areas; +} + + +void CPhysCollideCompactSurface::ComputeOrthographicAreas( float epsilon ) +{ + Vector mins, maxs, areas; + + physcollision->CollideGetAABB( &mins, &maxs, this, vec3_origin, vec3_angle ); + float side = sqrt( epsilon ); + if ( side < 1e-4f ) + side = 1e-4f; + Vector size = maxs-mins; + + m_orthoAreas.Init(1,1,1); + trace_t tr; + for ( int axis = 0; axis < 3; axis++ ) + { + int u = (axis+1)%3; + int v = (axis+2)%3; + int hits = 0; + int total = 0; + float halfSide = side * 0.5; + for ( float u0 = mins[u] + halfSide; u0 < maxs[u]; u0 += side ) + { + for ( float v0 = mins[v] + halfSide; v0 < maxs[v]; v0 += side ) + { + Vector start, end; + start[axis] = mins[axis]-1; + end[axis] = maxs[axis]+1; + start[u] = u0; + end[u] = u0; + start[v] = v0; + end[v] = v0; + + physcollision->TraceBox( start, end, vec3_origin, vec3_origin, this, vec3_origin, vec3_angle, &tr ); + if ( tr.DidHit() ) + { + hits++; + } + total++; + } + } + + if ( total <= 0 ) + total = 1; + m_orthoAreas[axis] = (float)hits / (float)total; + } +} + + +void CPhysCollideCompactSurface::ComputeHullInfo_r( hullinfo_t *pOut, const IVP_Compact_Ledgetree_Node *node ) const +{ + if ( !node->is_terminal() ) + { + if ( node->get_compact_hull() ) + pOut->hasOuterHull = true; + + ComputeHullInfo_r( pOut, node->left_son() ); + ComputeHullInfo_r( pOut, node->right_son() ); + } + else + { + // terminal node, add one ledge + pOut->convexCount++; + } +} + + +void CPhysCollideCompactSurface::OutputDebugInfo() const +{ + hullinfo_t info; + + ComputeHullInfo_r( &info, m_pCompactSurface->get_compact_ledge_tree_root() ); + const char *pOuterHull = info.hasOuterHull ? "with" : "no"; + Msg("CollisionModel: Compact Surface: %d convex pieces %s outer hull\n", info.convexCount, pOuterHull ); +} + +//----------------------------------------------------------------------------- + + +//----------------------------------------------------------------------------- +// Purpose: Create a convex element from a point cloud +// Input : **pVerts - array of points +// vertCount - length of array +// Output : opaque pointer to convex element +//----------------------------------------------------------------------------- +CPhysConvex *CPhysicsCollision::ConvexFromVertsFast( Vector **pVerts, int vertCount ) +{ + IVP_U_Vector<IVP_U_Point> points; + int i; + + for ( i = 0; i < vertCount; i++ ) + { + IVP_U_Point *tmp = new IVP_U_Point; + + ConvertPositionToIVP( *pVerts[i], *tmp ); + + BEGIN_IVP_ALLOCATION(); + points.add( tmp ); + END_IVP_ALLOCATION(); + } + + BEGIN_IVP_ALLOCATION(); + IVP_Compact_Ledge *pLedge = IVP_SurfaceBuilder_Pointsoup::convert_pointsoup_to_compact_ledge( &points ); + END_IVP_ALLOCATION(); + + for ( i = 0; i < points.len(); i++ ) + { + delete points.element_at(i); + } + points.clear(); + + return reinterpret_cast<CPhysConvex *>(pLedge); +} + +CPhysConvex *CPhysicsCollision::RebuildConvexFromPlanes( CPhysConvex *pConvex, float mergeTolerance ) +{ + if ( !pConvex ) + return NULL; + + IVP_Compact_Ledge *pLedge = (IVP_Compact_Ledge *)pConvex; + int triangleCount = pLedge->get_n_triangles(); + + IVP_Compact_Triangle *pTri = pLedge->get_first_triangle(); + IVP_U_Hesse plane; + IVP_Halfspacesoup halfspaces; + + for ( int j = 0; j < triangleCount; j++ ) + { + const IVP_Compact_Edge *pEdge = pTri->get_edge( 0 ); + const IVP_U_Float_Point *p0 = IVP_Compact_Ledge_Solver::give_object_coords(pEdge, pLedge); + const IVP_U_Float_Point *p2 = IVP_Compact_Ledge_Solver::give_object_coords(pEdge->get_next(), pLedge); + const IVP_U_Float_Point *p1 = IVP_Compact_Ledge_Solver::give_object_coords(pEdge->get_prev(), pLedge); + plane.calc_hesse(p0, p2, p1); + float testLen = plane.real_length(); + // if the triangle is less than 1mm on each side then skip it + if ( testLen > 1e-6f ) + { + plane.normize(); + halfspaces.add_halfspace( &plane ); + } + + pTri = pTri->get_next_tri(); + } + + IVP_Compact_Ledge *pLedgeOut = IVP_SurfaceBuilder_Halfspacesoup::convert_halfspacesoup_to_compact_ledge( &halfspaces, mergeTolerance ); + return reinterpret_cast<CPhysConvex *>( pLedgeOut ); +} + +CPhysConvex *CPhysicsCollision::ConvexFromVerts( Vector **pVerts, int vertCount ) +{ + CPhysConvex *pConvex = ConvexFromVertsFast( pVerts, vertCount ); + CPhysConvex *pReturn = RebuildConvexFromPlanes( pConvex, 0.01f ); // remove interior coplanar verts! + if ( pReturn ) + { + ConvexFree( pConvex ); + return pReturn; + } + return pConvex; +} + +// produce a convex element from planes (csg of planes) +CPhysConvex *CPhysicsCollision::ConvexFromPlanes( float *pPlanes, int planeCount, float mergeDistance ) +{ + // NOTE: We're passing in planes with outward-facing normals + // Ipion expects inward facing ones; we'll need to reverse plane directon + struct listplane_t + { + float normal[3]; + float dist; + }; + + listplane_t *pList = (listplane_t *)pPlanes; + IVP_U_Hesse plane; + IVP_Halfspacesoup halfspaces; + + mergeDistance = ConvertDistanceToIVP( mergeDistance ); + + for ( int i = 0; i < planeCount; i++ ) + { + Vector tmp( -pList[i].normal[0], -pList[i].normal[1], -pList[i].normal[2] ); + ConvertPlaneToIVP( tmp, -pList[i].dist, plane ); + halfspaces.add_halfspace( &plane ); + } + + IVP_Compact_Ledge *pLedge = IVP_SurfaceBuilder_Halfspacesoup::convert_halfspacesoup_to_compact_ledge( &halfspaces, mergeDistance ); + return reinterpret_cast<CPhysConvex *>( pLedge ); +} + + + +CPhysConvex *CPhysicsCollision::ConvexFromConvexPolyhedron( const CPolyhedron &ConvexPolyhedron ) +{ + IVP_Template_Polygon polyTemplate(ConvexPolyhedron.iVertexCount, ConvexPolyhedron.iLineCount, ConvexPolyhedron.iPolygonCount ); + + //convert/copy coordinates + for( int i = 0; i != ConvexPolyhedron.iVertexCount; ++i ) + ConvertPositionToIVP( ConvexPolyhedron.pVertices[i], polyTemplate.points[i] ); + + //copy lines + for( int i = 0; i != ConvexPolyhedron.iLineCount; ++i ) + polyTemplate.lines[i].set( ConvexPolyhedron.pLines[i].iPointIndices[0], ConvexPolyhedron.pLines[i].iPointIndices[1] ); + + //copy polygons + for( int i = 0; i != ConvexPolyhedron.iPolygonCount; ++i ) + { + polyTemplate.surfaces[i].init_surface( ConvexPolyhedron.pPolygons[i].iIndexCount ); //num vertices in a convex polygon == num lines + polyTemplate.surfaces[i].templ_poly = &polyTemplate; + + ConvertPositionToIVP( ConvexPolyhedron.pPolygons[i].polyNormal, polyTemplate.surfaces[i].normal ); + + Polyhedron_IndexedLineReference_t *pLineReferences = &ConvexPolyhedron.pIndices[ConvexPolyhedron.pPolygons[i].iFirstIndex]; + for( int j = 0; j != ConvexPolyhedron.pPolygons[i].iIndexCount; ++j ) + { + polyTemplate.surfaces[i].lines[j] = pLineReferences[j].iLineIndex; + polyTemplate.surfaces[i].revert_line[j] = pLineReferences[j].iEndPointIndex; + } + } + + //final conversion + IVP_Compact_Ledge *pLedge = IVP_SurfaceBuilder_Polygon_Convex::convert_template_to_ledge(&polyTemplate); + + //cleanup + for( int i = 0; i != ConvexPolyhedron.iPolygonCount; ++i ) + polyTemplate.surfaces[i].close_surface(); + + return reinterpret_cast<CPhysConvex *>(pLedge); +} + + + + + +struct PolyhedronMesh_Triangle +{ + struct + { + int iPointIndices[2]; + } Edges[3]; +}; + + + +//TODO: Optimize the returned polyhedron to get away from the triangulated mesh +CPolyhedron *CPhysicsCollision::PolyhedronFromConvex( CPhysConvex * const pConvex, bool bUseTempPolyhedron ) +{ + IVP_Compact_Ledge *pLedge = (IVP_Compact_Ledge *)pConvex; + int iTriangles = pLedge->get_n_triangles(); + + PolyhedronMesh_Triangle *pTriangles = (PolyhedronMesh_Triangle *)stackalloc( iTriangles * sizeof( PolyhedronMesh_Triangle ) ); + + int iHighestPointIndex = 0; + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle(); + for( int i = 0; i != iTriangles; ++i ) + { + //reverse point ordering while creating edges + pTriangles[i].Edges[2].iPointIndices[1] = pTriangles[i].Edges[0].iPointIndices[0] = pTri->get_edge( 2 )->get_start_point_index(); + pTriangles[i].Edges[0].iPointIndices[1] = pTriangles[i].Edges[1].iPointIndices[0] = pTri->get_edge( 1 )->get_start_point_index(); + pTriangles[i].Edges[1].iPointIndices[1] = pTriangles[i].Edges[2].iPointIndices[0] = pTri->get_edge( 0 )->get_start_point_index(); + + for( int j = 0; j != 3; ++j ) + { + //get_n_points() has a whole bunch of ifdefs that apparently disable it in this case, detect number of points + if( pTriangles[i].Edges[j].iPointIndices[0] > iHighestPointIndex ) + iHighestPointIndex = pTriangles[i].Edges[j].iPointIndices[0]; + } + + pTri = pTri->get_next_tri(); + } + + ++iHighestPointIndex; + + //apparently points might be shared between ledges and not all points will be used. So now we get to compress them into a smaller set + int *pPointRemapping = (int *)stackalloc( iHighestPointIndex * sizeof( int ) ); + memset( pPointRemapping, 0, iHighestPointIndex * sizeof( int ) ); + for( int i = 0; i != iTriangles; ++i ) + { + for( int j = 0; j != 3; ++j ) + ++(pPointRemapping[pTriangles[i].Edges[j].iPointIndices[0]]); + } + + int iInsertIndex = 0; + + for( int i = 0; i != iHighestPointIndex; ++i ) + { + if( pPointRemapping[i] ) + { + pPointRemapping[i] = iInsertIndex; + ++iInsertIndex; + } + else + { + pPointRemapping[i] = -1; + } + } + + const int iNumPoints = iInsertIndex; + + for( int i = 0; i != iTriangles; ++i ) + { + for( int j = 0; j != 3; ++j ) + { + for( int k = 0; k != 2; ++k ) + pTriangles[i].Edges[j].iPointIndices[k] = pPointRemapping[pTriangles[i].Edges[j].iPointIndices[k]]; + } + } + + + bool *bLinks = (bool *)stackalloc( iNumPoints * iNumPoints * sizeof( bool ) ); + memset( bLinks, 0, iNumPoints * iNumPoints * sizeof( bool ) ); + + int iLinkCount = 0; + for( int i = 0; i != iTriangles; ++i ) + { + for( int j = 0; j != 3; ++j ) + { + const int *pIndices = pTriangles[i].Edges[j].iPointIndices; + int iLow = ((pIndices[0] > pIndices[1])?1:(0)); + ++iLinkCount; //this will technically make the link count double the actual number + bLinks[(pIndices[iLow] * iNumPoints) + pIndices[1-iLow]] = true; + } + } + + iLinkCount /= 2; //cut the link count in half since we overcounted + + CPolyhedron *pReturn; + if( bUseTempPolyhedron ) + pReturn = GetTempPolyhedron( iNumPoints, iLinkCount, iLinkCount * 2, iTriangles ); + else + pReturn = CPolyhedron_AllocByNew::Allocate( iNumPoints, iLinkCount, iLinkCount * 2, iTriangles ); + + //copy/convert vertices + const IVP_Compact_Poly_Point *pLedgePoints = pLedge->get_point_array(); + Vector *pWriteVertices = pReturn->pVertices; + for( int i = 0; i != iHighestPointIndex; ++i ) + { + if( pPointRemapping[i] != -1 ) + ConvertPositionToHL( pLedgePoints[i], pWriteVertices[pPointRemapping[i]] ); + } + + + //convert lines + iInsertIndex = 0; + for( int i = 0; i != iNumPoints; ++i ) + { + for( int j = i + 1; j != iNumPoints; ++j ) + { + if( bLinks[(i * iNumPoints) + j] ) + { + pReturn->pLines[iInsertIndex].iPointIndices[0] = i; + pReturn->pLines[iInsertIndex].iPointIndices[1] = j; + ++iInsertIndex; + } + } + } + + + int *pStartIndices = (int *)stackalloc( iNumPoints * sizeof( int ) ); //for quicker lookup of which edges to use in polygons + + pStartIndices[0] = 0; //the lowest index point drives links, so if the first point isn't the first link, then something is extremely messed up + Assert( pReturn->pLines[0].iPointIndices[0] == 0 ); + iInsertIndex = 1; + for( int i = 1; i != iNumPoints; ++i ) + { + for( int j = iInsertIndex; j != iLinkCount; ++j ) + { + if( pReturn->pLines[j].iPointIndices[0] == i ) + { + pStartIndices[i] = j; + iInsertIndex = j + 1; + break; + } + } + } + + //convert polygons and setup line references as a subtask + iInsertIndex = 0; + for( int i = 0; i != iTriangles; ++i ) + { + pReturn->pPolygons[i].iFirstIndex = iInsertIndex; + pReturn->pPolygons[i].iIndexCount = 3; + + Vector *p1, *p2, *p3; + p1 = &pReturn->pVertices[pTriangles[i].Edges[0].iPointIndices[0]]; + p2 = &pReturn->pVertices[pTriangles[i].Edges[1].iPointIndices[0]]; + p3 = &pReturn->pVertices[pTriangles[i].Edges[2].iPointIndices[0]]; + + Vector v1to2, v1to3; + + v1to2 = *p2 - *p1; + v1to3 = *p3 - *p1; + + pReturn->pPolygons[i].polyNormal = v1to3.Cross( v1to2 ); + pReturn->pPolygons[i].polyNormal.NormalizeInPlace(); + + for( int j = 0; j != 3; ++j, ++iInsertIndex ) + { + const int *pIndices = pTriangles[i].Edges[j].iPointIndices; + int iLow = (pIndices[0] > pIndices[1])?1:0; + int iLineIndex; + for( iLineIndex = pStartIndices[pIndices[iLow]]; iLineIndex != iLinkCount; ++iLineIndex ) + { + if( (pReturn->pLines[iLineIndex].iPointIndices[0] == pIndices[iLow]) && + (pReturn->pLines[iLineIndex].iPointIndices[1] == pIndices[1 - iLow]) ) + { + break; + } + } + + pReturn->pIndices[iInsertIndex].iLineIndex = iLineIndex; + pReturn->pIndices[iInsertIndex].iEndPointIndex = 1 - iLow; + } + } + + return pReturn; +} + + +int CPhysicsCollision::GetConvexesUsedInCollideable( const CPhysCollide *pCollideable, CPhysConvex **pOutputArray, int iOutputArrayLimit ) +{ + IVP_U_BigVector<IVP_Compact_Ledge> ledges; + pCollideable->GetAllLedges( ledges ); + + int iLedgeCount = ledges.len(); + if( iLedgeCount > iOutputArrayLimit ) + iLedgeCount = iOutputArrayLimit; + + for( int i = 0; i != iLedgeCount; ++i ) + { + IVP_Compact_Ledge *pLedge = ledges.element_at(i); //doing as a 2 step since a single convert seems more error prone (without compile error) in this case + pOutputArray[i] = (CPhysConvex *)pLedge; + } + + return iLedgeCount; +} + +void CPhysicsCollision::ConvexesFromConvexPolygon( const Vector &vPolyNormal, const Vector *pPoints, int iPointCount, CPhysConvex **pOutput ) +{ + IVP_U_Point *pIVP_Points = (IVP_U_Point *)stackalloc( sizeof( IVP_U_Point ) * iPointCount ); + IVP_U_Point **pTriangulator = (IVP_U_Point **)stackalloc( sizeof( IVP_U_Point * ) * iPointCount ); + IVP_U_Point **pRead = pTriangulator; + IVP_U_Point **pWrite = pTriangulator; + + //convert coordinates + { + for( int i = 0; i != iPointCount; ++i ) + ConvertPositionToIVP( pPoints[i], pIVP_Points[i] ); + } + + int iOutputCount = 0; + + //chunk this out like a triangle strip + int iForwardCounter = 1; + int iReverseCounter = iPointCount - 1; //guaranteed to be >= 2 to start + + *pWrite = &pIVP_Points[0]; + ++pWrite; + *pWrite = &pIVP_Points[iReverseCounter]; + ++pWrite; + --iReverseCounter; + + do + { + //forward + *pWrite = &pIVP_Points[iForwardCounter]; + ++iForwardCounter; + + pOutput[iOutputCount] = reinterpret_cast<CPhysConvex *>(IVP_SurfaceBuilder_Pointsoup::convert_triangle_to_compace_ledge( pRead[0], pRead[1], pRead[2] )); + Assert( pOutput[iOutputCount] ); + ++iOutputCount; + if( iForwardCounter > iReverseCounter ) + break; + + ++pRead; + ++pWrite; + + + + //backward + *pWrite = &pIVP_Points[iReverseCounter]; + --iReverseCounter; + + pOutput[iOutputCount] = reinterpret_cast<CPhysConvex *>(IVP_SurfaceBuilder_Pointsoup::convert_triangle_to_compace_ledge( pRead[0], pRead[1], pRead[2] )); + Assert( pOutput[iOutputCount] ); + ++iOutputCount; + + if( iForwardCounter > iReverseCounter ) + break; + + ++pRead; + ++pWrite; + } while( true ); +} + + +//----------------------------------------------------------------------------- +// Purpose: copies the first vert int pLedge to out +// Input : *pLedge - compact ledge +// *out - destination float array for the vert +//----------------------------------------------------------------------------- +static void LedgeInsidePoint( IVP_Compact_Ledge *pLedge, Vector& out ) +{ + IVP_Compact_Triangle *pTri = pLedge->get_first_triangle(); + const IVP_Compact_Edge *pEdge = pTri->get_edge( 0 ); + const IVP_U_Float_Point *pPoint = pEdge->get_start_point( pLedge ); + ConvertPositionToHL( *pPoint, out ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Calculate the volume of a tetrahedron with these vertices +// Input : p0 - points of tetrahedron +// p1 - +// p2 - +// p3 - +// Output : float (volume in units^3) +//----------------------------------------------------------------------------- +static float TetrahedronVolume( const Vector &p0, const Vector &p1, const Vector &p2, const Vector &p3 ) +{ + Vector a, b, c, cross; + float volume = 1.0f / 6.0f; + + a = p1 - p0; + b = p2 - p0; + c = p3 - p0; + cross = CrossProduct( b, c ); + + volume *= DotProduct( a, cross ); + if ( volume < 0 ) + return -volume; + return volume; +} + + +static float TriangleArea( const Vector &p0, const Vector &p1, const Vector &p2 ) +{ + Vector e0 = p1 - p0; + Vector e1 = p2 - p0; + Vector cross; + + CrossProduct( e0, e1, cross ); + return 0.5 * cross.Length(); +} + + +//----------------------------------------------------------------------------- +// Purpose: Tetrahedronalize this ledge and compute it's volume in BSP space +// Input : convex - the ledge +// Output : float - volume in HL units (in^3) +//----------------------------------------------------------------------------- +float CPhysicsCollision::ConvexVolume( CPhysConvex *pConvex ) +{ + IVP_Compact_Ledge *pLedge = (IVP_Compact_Ledge *)pConvex; + int triangleCount = pLedge->get_n_triangles(); + + IVP_Compact_Triangle *pTri = pLedge->get_first_triangle(); + + Vector vert; + float volume = 0; + // vert is in HL units + LedgeInsidePoint( pLedge, vert ); + + for ( int j = 0; j < triangleCount; j++ ) + { + Vector points[3]; + for ( int k = 0; k < 3; k++ ) + { + const IVP_Compact_Edge *pEdge = pTri->get_edge( k ); + const IVP_U_Float_Point *pPoint = pEdge->get_start_point( pLedge ); + ConvertPositionToHL( *pPoint, points[k] ); + } + volume += TetrahedronVolume( vert, points[0], points[1], points[2] ); + + pTri = pTri->get_next_tri(); + } + + return volume; +} + + +float CPhysicsCollision::ConvexSurfaceArea( CPhysConvex *pConvex ) +{ + IVP_Compact_Ledge *pLedge = (IVP_Compact_Ledge *)pConvex; + int triangleCount = pLedge->get_n_triangles(); + + IVP_Compact_Triangle *pTri = pLedge->get_first_triangle(); + + float area = 0; + + for ( int j = 0; j < triangleCount; j++ ) + { + Vector points[3]; + for ( int k = 0; k < 3; k++ ) + { + const IVP_Compact_Edge *pEdge = pTri->get_edge( k ); + const IVP_U_Float_Point *pPoint = pEdge->get_start_point( pLedge ); + ConvertPositionToHL( *pPoint, points[k] ); + } + area += TriangleArea( points[0], points[1], points[2] ); + + pTri = pTri->get_next_tri(); + } + + return area; +} + +// Convert an array of convex elements to a compiled collision model (this deletes the convex elements) +CPhysCollide *CPhysicsCollision::ConvertConvexToCollide( CPhysConvex **pConvex, int convexCount ) +{ + convertconvexparams_t convertParams; + convertParams.Defaults(); + return ConvertConvexToCollideParams( pConvex, convexCount, convertParams ); +} + +CPhysCollide *CPhysicsCollision::ConvertConvexToCollideParams( CPhysConvex **pConvex, int convexCount, const convertconvexparams_t &convertParams ) +{ + if ( !convexCount || !pConvex ) + return NULL; + + int validConvex = 0; + BEGIN_IVP_ALLOCATION(); + IVP_SurfaceBuilder_Ledge_Soup builder; + IVP_Compact_Surface *pSurface = NULL; + + for ( int i = 0; i < convexCount; i++ ) + { + if ( pConvex[i] ) + { + validConvex++; + builder.insert_ledge( (IVP_Compact_Ledge *)pConvex[i] ); + } + } + // if the outside code does something stupid, don't crash + if ( validConvex ) + { + IVP_Template_Surbuild_LedgeSoup params; + params.force_convex_hull = (IVP_Compact_Ledge *)convertParams.pForcedOuterHull; + params.build_root_convex_hull = convertParams.buildOuterConvexHull ? IVP_TRUE : IVP_FALSE; + + // NOTE: THIS FREES THE LEDGES in pConvex!!! + pSurface = builder.compile( ¶ms ); + CPhysCollide *pCollide = new CPhysCollideCompactSurface( pSurface ); + if ( convertParams.buildDragAxisAreas ) + { + pCollide->ComputeOrthographicAreas( convertParams.dragAreaEpsilon ); + } + + END_IVP_ALLOCATION(); + return pCollide; + } + + END_IVP_ALLOCATION(); + + return NULL; +} + +static void InitBoxVerts( Vector *boxVerts, Vector **ppVerts, const Vector &mins, const Vector &maxs ) +{ + for (int i = 0; i < 8; ++i) + { + boxVerts[i][0] = (i & 0x1) ? maxs[0] : mins[0]; + boxVerts[i][1] = (i & 0x2) ? maxs[1] : mins[1]; + boxVerts[i][2] = (i & 0x4) ? maxs[2] : mins[2]; + if ( ppVerts ) + { + ppVerts[i] = &boxVerts[i]; + } + } +} + + +#define FAST_BBOX 1 +CPhysCollideCompactSurface *CPhysicsCollision::FastBboxCollide( const CPhysCollideCompactSurface *pCollide, const Vector &mins, const Vector &maxs ) +{ + Vector boxVerts[8]; + InitBoxVerts( boxVerts, NULL, mins, maxs ); + // copy the compact ledge at bboxCache 0 + // stuff the verts in there + const IVP_Compact_Surface *pSurface = ConvertPhysCollideToCompactSurface( pCollide ); + Assert( pSurface ); + const IVP_Compact_Ledgetree_Node *node = pSurface->get_compact_ledge_tree_root(); + Assert( node->is_terminal() == IVP_TRUE ); + const IVP_Compact_Ledge *pLedge = node->get_compact_ledge(); + int ledgeSize = pLedge->get_size(); + IVP_Compact_Ledge *pNewLedge = (IVP_Compact_Ledge *)ivp_malloc_aligned( ledgeSize, 16 ); + memcpy( pNewLedge, pLedge, ledgeSize ); + pNewLedge->set_client_data(0); + IVP_Compact_Poly_Point *pPoints = pNewLedge->get_point_array(); + for ( int i = 0; i < 8; i++ ) + { + IVP_U_Float_Hesse ivp; + ConvertPositionToIVP( boxVerts[m_bboxVertMap[i]], ivp ); + ivp.hesse_val = 0; + pPoints[i].set4(&ivp); + } + CPhysConvex *pConvex = (CPhysConvex *)pNewLedge; + return (CPhysCollideCompactSurface *)ConvertConvexToCollide( &pConvex, 1 ); +} + +void CPhysicsCollision::InitBBoxCache() +{ + Vector boxVerts[8], *ppVerts[8]; + Vector mins(-16,-16,0), maxs(16,16,72); + // init with the player box + InitBoxVerts( boxVerts, ppVerts, mins, maxs ); + // Generate a convex hull from the verts + CPhysConvex *pConvex = ConvexFromVertsFast( ppVerts, 8 ); + IVP_Compact_Poly_Point *pPoints = reinterpret_cast<IVP_Compact_Ledge *>(pConvex)->get_point_array(); + for ( int i = 0; i < 8; i++ ) + { + int nearest = -1; + float minDist = 0.1; + Vector tmp; + ConvertPositionToHL( pPoints[i], tmp ); + for ( int j = 0; j < 8; j++ ) + { + float dist = (boxVerts[j] - tmp).Length(); + if ( dist < minDist ) + { + minDist = dist; + nearest = j; + } + } + + m_bboxVertMap[i] = nearest; + +#if _DEBUG + for ( int k = 0; k < i; k++ ) + { + Assert( m_bboxVertMap[k] != m_bboxVertMap[i] ); + } +#endif + // NOTE: If this is wrong, you can disable FAST_BBOX above to fix + AssertMsg( nearest != -1, "CPhysCollide: Vert map is wrong\n" ); + } + CPhysCollide *pCollide = ConvertConvexToCollide( &pConvex, 1 ); + AddBBoxCache( (CPhysCollideCompactSurface *)pCollide, mins, maxs ); +} + + +CPhysConvex *CPhysicsCollision::BBoxToConvex( const Vector &mins, const Vector &maxs ) +{ + Vector boxVerts[8], *ppVerts[8]; + InitBoxVerts( boxVerts, ppVerts, mins, maxs ); + // Generate a convex hull from the verts + return ConvexFromVertsFast( ppVerts, 8 ); +} + +CPhysCollide *CPhysicsCollision::BBoxToCollide( const Vector &mins, const Vector &maxs ) +{ + // can't create a collision model for an empty box ! + if ( mins == maxs ) + { + Assert(0); + return NULL; + } + + // find this bbox in the cache + CPhysCollide *pCollide = GetBBoxCache( mins, maxs ); + if ( pCollide ) + return pCollide; + + // FAST_BBOX: uses an existing compact ledge as a template for fast generation + // building convex hulls from points is slow +#if FAST_BBOX + if ( m_bboxCache.Count() == 0 ) + { + InitBBoxCache(); + } + pCollide = FastBboxCollide( m_bboxCache[0].pCollide, mins, maxs ); +#else + CPhysConvex *pConvex = BBoxToConvex( mins, maxs ); + pCollide = ConvertConvexToCollide( &pConvex, 1 ); +#endif + AddBBoxCache( (CPhysCollideCompactSurface *)pCollide, mins, maxs ); + return pCollide; +} + +bool CPhysicsCollision::IsBBoxCache( CPhysCollide *pCollide ) +{ + // UNDONE: Sort the list so it can be searched spatially instead of linearly? + for ( int i = m_bboxCache.Count()-1; i >= 0; i-- ) + { + if ( m_bboxCache[i].pCollide == pCollide ) + return true; + } + return false; +} + +void CPhysicsCollision::AddBBoxCache( CPhysCollideCompactSurface *pCollide, const Vector &mins, const Vector &maxs ) +{ + int index = m_bboxCache.AddToTail(); + bboxcache_t *pCache = &m_bboxCache[index]; + pCache->pCollide = pCollide; + pCache->mins = mins; + pCache->maxs = maxs; +} + +CPhysCollideCompactSurface *CPhysicsCollision::GetBBoxCache( const Vector &mins, const Vector &maxs ) +{ + for ( int i = m_bboxCache.Count()-1; i >= 0; i-- ) + { + if ( m_bboxCache[i].mins == mins && m_bboxCache[i].maxs == maxs ) + return m_bboxCache[i].pCollide; + } + return NULL; +} + + +void CPhysicsCollision::ConvexFree( CPhysConvex *pConvex ) +{ + if ( !pConvex ) + return; + ivp_free_aligned( pConvex ); +} + +// Get the size of the collision model for serialization +int CPhysicsCollision::CollideSize( CPhysCollide *pCollide ) +{ + return pCollide->GetSerializationSize(); +} + +int CPhysicsCollision::CollideWrite( char *pDest, CPhysCollide *pCollide, bool bSwap ) +{ + return pCollide->SerializeToBuffer( pDest, bSwap ); +} + +CPhysCollide *CPhysicsCollision::UnserializeCollide( char *pBuffer, int size, int index ) +{ + return CPhysCollide::UnserializeFromBuffer( pBuffer, size, index ); +} + +class CPhysPolysoup +{ +public: + CPhysPolysoup(); +#if ENABLE_IVP_MOPP + IVP_SurfaceBuilder_Mopp m_builder; +#endif + IVP_SurfaceBuilder_Ledge_Soup m_builderSoup; + IVP_U_Vector<IVP_U_Point> m_points; + IVP_U_Point m_triangle[3]; + + bool m_isValid; +}; + +CPhysPolysoup::CPhysPolysoup() +{ + m_isValid = false; + m_points.add( &m_triangle[0] ); + m_points.add( &m_triangle[1] ); + m_points.add( &m_triangle[2] ); +} + +CPhysPolysoup *CPhysicsCollision::PolysoupCreate( void ) +{ + return new CPhysPolysoup; +} + +void CPhysicsCollision::PolysoupDestroy( CPhysPolysoup *pSoup ) +{ + delete pSoup; +} + +void CPhysicsCollision::PolysoupAddTriangle( CPhysPolysoup *pSoup, const Vector &a, const Vector &b, const Vector &c, int materialIndex7bits ) +{ + pSoup->m_isValid = true; + ConvertPositionToIVP( a, pSoup->m_triangle[0] ); + ConvertPositionToIVP( b, pSoup->m_triangle[1] ); + ConvertPositionToIVP( c, pSoup->m_triangle[2] ); + IVP_Compact_Ledge *pLedge = IVP_SurfaceBuilder_Pointsoup::convert_pointsoup_to_compact_ledge(&pSoup->m_points); + if ( !pLedge ) + { + Warning("Degenerate Triangle\n"); + Warning("(%.2f, %.2f, %.2f), ", a.x, a.y, a.z ); + Warning("(%.2f, %.2f, %.2f), ", b.x, b.y, b.z ); + Warning("(%.2f, %.2f, %.2f)\n", c.x, c.y, c.z ); + return; + } + IVP_Compact_Triangle *pTriangle = pLedge->get_first_triangle(); + pTriangle->set_material_index( materialIndex7bits ); +#if ENABLE_IVP_MOPP + pSoup->m_builder.insert_ledge(pLedge); +#endif + pSoup->m_builderSoup.insert_ledge(pLedge); +} + +CPhysCollide *CPhysicsCollision::ConvertPolysoupToCollide( CPhysPolysoup *pSoup, bool useMOPP ) +{ + if ( !pSoup->m_isValid ) + return NULL; + + CPhysCollide *pCollide = NULL; +#if ENABLE_IVP_MOPP + if ( useMOPP ) + { + IVP_Compact_Mopp *pSurface = pSoup->m_builder.compile(); + pCollide = new CPhysCollideMopp( pSurface ); + } + else +#endif + { + IVP_Compact_Surface *pSurface = pSoup->m_builderSoup.compile(); + pCollide = new CPhysCollideCompactSurface( pSurface ); + } + + Assert(pCollide); + + // There's a bug in IVP where the duplicated triangles (for 2D) + // don't get the materials set properly, so copy them + IVP_U_BigVector<IVP_Compact_Ledge> ledges; + pCollide->GetAllLedges( ledges ); + + for ( int i = 0; i < ledges.len(); i++ ) + { + IVP_Compact_Ledge *pLedge = ledges.element_at( i ); + int triangleCount = pLedge->get_n_triangles(); + + IVP_Compact_Triangle *pTri = pLedge->get_first_triangle(); + int materialIndex = pTri->get_material_index(); + if ( !materialIndex ) + { + for ( int j = 0; j < triangleCount; j++ ) + { + if ( pTri->get_material_index() != 0 ) + { + materialIndex = pTri->get_material_index(); + } + pTri = pTri->get_next_tri(); + } + } + for ( int j = 0; j < triangleCount; j++ ) + { + pTri->set_material_index( materialIndex ); + pTri = pTri->get_next_tri(); + } + } + + return pCollide; +} + +int CPhysicsCollision::CreateDebugMesh( const CPhysCollide *pCollisionModel, Vector **outVerts ) +{ + int i; + + IVP_U_BigVector<IVP_Compact_Ledge> ledges; + pCollisionModel->GetAllLedges( ledges ); + + int vertCount = 0; + + for ( i = 0; i < ledges.len(); i++ ) + { + IVP_Compact_Ledge *pLedge = ledges.element_at( i ); + vertCount += pLedge->get_n_triangles() * 3; + } + Vector *verts = new Vector[ vertCount ]; + + int vertIndex = 0; + for ( i = 0; i < ledges.len(); i++ ) + { + IVP_Compact_Ledge *pLedge = ledges.element_at( i ); + int triangleCount = pLedge->get_n_triangles(); + + IVP_Compact_Triangle *pTri = pLedge->get_first_triangle(); + for ( int j = 0; j < triangleCount; j++ ) + { + for ( int k = 2; k >= 0; k-- ) + { + const IVP_Compact_Edge *pEdge = pTri->get_edge( k ); + const IVP_U_Float_Point *pPoint = pEdge->get_start_point( pLedge ); + + Vector* pVec = verts + vertIndex; + ConvertPositionToHL( *pPoint, *pVec ); + vertIndex++; + } + pTri = pTri->get_next_tri(); + } + } + + *outVerts = verts; + return vertCount; +} + + +void CPhysicsCollision::DestroyDebugMesh( int vertCount, Vector *outVerts ) +{ + delete[] outVerts; +} + + +void CPhysicsCollision::SetConvexGameData( CPhysConvex *pConvex, unsigned int gameData ) +{ + IVP_Compact_Ledge *pLedge = reinterpret_cast<IVP_Compact_Ledge *>( pConvex ); + pLedge->set_client_data( gameData ); +} + + +void CPhysicsCollision::TraceBox( const Vector &start, const Vector &end, const Vector &mins, const Vector &maxs, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, trace_t *ptr ) +{ + m_traceapi.SweepBoxIVP( start, end, mins, maxs, pCollide, collideOrigin, collideAngles, ptr ); +} + +void CPhysicsCollision::TraceBox( const Ray_t &ray, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, trace_t *ptr ) +{ + TraceBox( ray, MASK_ALL, NULL, pCollide, collideOrigin, collideAngles, ptr ); +} + +void CPhysicsCollision::TraceBox( const Ray_t &ray, unsigned int contentsMask, IConvexInfo *pConvexInfo, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, trace_t *ptr ) +{ + m_traceapi.SweepBoxIVP( ray, contentsMask, pConvexInfo, pCollide, collideOrigin, collideAngles, ptr ); +} + +// Trace one collide against another +void CPhysicsCollision::TraceCollide( const Vector &start, const Vector &end, const CPhysCollide *pSweepCollide, const QAngle &sweepAngles, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, trace_t *ptr ) +{ + m_traceapi.SweepIVP( start, end, pSweepCollide, sweepAngles, pCollide, collideOrigin, collideAngles, ptr ); +} + +void CPhysicsCollision::CollideGetAABB( Vector *pMins, Vector *pMaxs, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles ) +{ + m_traceapi.GetAABB( pMins, pMaxs, pCollide, collideOrigin, collideAngles ); +} + + +Vector CPhysicsCollision::CollideGetExtent( const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, const Vector &direction ) +{ + if ( !pCollide ) + return collideOrigin; + + return m_traceapi.GetExtent( pCollide, collideOrigin, collideAngles, direction ); +} + +bool CPhysicsCollision::IsBoxIntersectingCone( const Vector &boxAbsMins, const Vector &boxAbsMaxs, const truncatedcone_t &cone ) +{ + return m_traceapi.IsBoxIntersectingCone( boxAbsMins, boxAbsMaxs, cone ); +} + +// Free a collide that was created with ConvertConvexToCollide() +void CPhysicsCollision::DestroyCollide( CPhysCollide *pCollide ) +{ + if ( !IsBBoxCache( pCollide ) ) + { + delete pCollide; + } +} + +// calculate the volume of a collide by calling ConvexVolume on its parts +float CPhysicsCollision::CollideVolume( CPhysCollide *pCollide ) +{ + IVP_U_BigVector<IVP_Compact_Ledge> ledges; + pCollide->GetAllLedges( ledges ); + + float volume = 0; + for ( int i = 0; i < ledges.len(); i++ ) + { + volume += ConvexVolume( (CPhysConvex *)ledges.element_at(i) ); + } + + return volume; +} + +// calculate the volume of a collide by calling ConvexVolume on its parts +float CPhysicsCollision::CollideSurfaceArea( CPhysCollide *pCollide ) +{ + IVP_U_BigVector<IVP_Compact_Ledge> ledges; + pCollide->GetAllLedges( ledges ); + + float area = 0; + for ( int i = 0; i < ledges.len(); i++ ) + { + area += ConvexSurfaceArea( (CPhysConvex *)ledges.element_at(i) ); + } + + return area; +} + + +// loads a set of solids into a vcollide_t +void CPhysicsCollision::VCollideLoad( vcollide_t *pOutput, int solidCount, const char *pBuffer, int bufferSize, bool swap ) +{ + memset( pOutput, 0, sizeof(*pOutput) ); + int position = 0; + + pOutput->solidCount = solidCount; + pOutput->solids = new CPhysCollide *[solidCount]; + + BEGIN_IVP_ALLOCATION(); + + for ( int i = 0; i < solidCount; i++ ) + { + int size; + memcpy( &size, pBuffer + position, sizeof(int) ); + position += sizeof(int); + + pOutput->solids[i] = CPhysCollide::UnserializeFromBuffer( pBuffer + position, size, i, swap ); + position += size; + } + + END_IVP_ALLOCATION(); + pOutput->isPacked = false; + int keySize = bufferSize - position; + pOutput->pKeyValues = new char[keySize]; + memcpy( pOutput->pKeyValues, pBuffer + position, keySize ); + pOutput->descSize = 0; +} + +// destroys the set of solids created by VCollideCreateCPhysCollide +void CPhysicsCollision::VCollideUnload( vcollide_t *pVCollide ) +{ + for ( int i = 0; i < pVCollide->solidCount; i++ ) + { +#if _DEBUG + // HACKHACK: 1024 is just "some big number" + // GetActiveEnvironmentByIndex() will eventually return NULL when there are no more environments. + // In HL2 & TF2, there are only 2 environments - so j > 1 is probably an error! + for ( int j = 0; j < 1024; j++ ) + { + IPhysicsEnvironment *pEnv = g_PhysicsInternal->GetActiveEnvironmentByIndex( j ); + if ( !pEnv ) + break; + + if ( pEnv->IsCollisionModelUsed( (CPhysCollide *)pVCollide->solids[i] ) ) + { + AssertMsg(0, "Freed collision model while in use!!!\n"); + return; + } + } +#endif + delete pVCollide->solids[i]; + } + delete[] pVCollide->solids; + delete[] pVCollide->pKeyValues; + memset( pVCollide, 0, sizeof(*pVCollide) ); +} + +// begins parsing a vcollide. NOTE: This keeps pointers to the vcollide_t +// If you delete the vcollide_t and call members of IVCollideParse, it will crash +IVPhysicsKeyParser *CPhysicsCollision::VPhysicsKeyParserCreate( const char *pKeyData ) +{ + return CreateVPhysicsKeyParser( pKeyData ); +} + +// Free the parser created by VPhysicsKeyParserCreate +void CPhysicsCollision::VPhysicsKeyParserDestroy( IVPhysicsKeyParser *pParser ) +{ + DestroyVPhysicsKeyParser( pParser ); +} + +IPhysicsCollision *CPhysicsCollision::ThreadContextCreate( void ) +{ + return this; +} + +void CPhysicsCollision::ThreadContextDestroy( IPhysicsCollision *pThreadContext ) +{ +} + + +void CPhysicsCollision::CollideGetMassCenter( CPhysCollide *pCollide, Vector *pOutMassCenter ) +{ + *pOutMassCenter = pCollide->GetMassCenter(); +} + +void CPhysicsCollision::CollideSetMassCenter( CPhysCollide *pCollide, const Vector &massCenter ) +{ + pCollide->SetMassCenter( massCenter ); +} + +int CPhysicsCollision::CollideIndex( const CPhysCollide *pCollide ) +{ + if ( !pCollide ) + return 0; + return pCollide->GetVCollideIndex(); +} + +Vector CPhysicsCollision::CollideGetOrthographicAreas( const CPhysCollide *pCollide ) +{ + if ( !pCollide ) + return vec3_origin; + return pCollide->GetOrthographicAreas(); +} + +void CPhysicsCollision::CollideSetOrthographicAreas( CPhysCollide *pCollide, const Vector &areas ) +{ + if ( pCollide ) + pCollide->SetOrthographicAreas( areas ); +} + +// returns true if this collide has an outer hull built +void CPhysicsCollision::OutputDebugInfo( const CPhysCollide *pCollide ) +{ + pCollide->OutputDebugInfo(); +} + +bool CPhysicsCollision::GetBBoxCacheSize( int *pCachedSize, int *pCachedCount ) +{ + *pCachedSize = 0; + *pCachedCount = m_bboxCache.Count(); + for ( int i = 0; i < *pCachedCount; i++ ) + { + *pCachedSize += m_bboxCache[i].pCollide->GetSerializationSize(); + } + return true; +} + +class CCollisionQuery : public ICollisionQuery +{ +public: + CCollisionQuery( CPhysCollide *pCollide ); + ~CCollisionQuery( void ) {} + + // number of convex pieces in the whole solid + virtual int ConvexCount( void ); + // triangle count for this convex piece + virtual int TriangleCount( int convexIndex ); + + // get the stored game data + virtual unsigned int GetGameData( int convexIndex ); + + // Gets the triangle's verts to an array + virtual void GetTriangleVerts( int convexIndex, int triangleIndex, Vector *verts ); + + // UNDONE: This doesn't work!!! + virtual void SetTriangleVerts( int convexIndex, int triangleIndex, const Vector *verts ); + + // returns the 7-bit material index + virtual int GetTriangleMaterialIndex( int convexIndex, int triangleIndex ); + // sets a 7-bit material index for this triangle + virtual void SetTriangleMaterialIndex( int convexIndex, int triangleIndex, int index7bits ); + +private: + IVP_Compact_Triangle *Triangle( IVP_Compact_Ledge *pLedge, int triangleIndex ); + + IVP_U_BigVector <IVP_Compact_Ledge> m_ledges; +}; + + +// create a queryable version of the collision model +ICollisionQuery *CPhysicsCollision::CreateQueryModel( CPhysCollide *pCollide ) +{ + return new CCollisionQuery( pCollide ); +} + + // destroy the queryable version +void CPhysicsCollision::DestroyQueryModel( ICollisionQuery *pQuery ) +{ + delete pQuery; +} + + +CCollisionQuery::CCollisionQuery( CPhysCollide *pCollide ) +{ + pCollide->GetAllLedges( m_ledges ); +} + + + // number of convex pieces in the whole solid +int CCollisionQuery::ConvexCount( void ) +{ + return m_ledges.len(); +} + + // triangle count for this convex piece +int CCollisionQuery::TriangleCount( int convexIndex ) +{ + IVP_Compact_Ledge *pLedge = m_ledges.element_at(convexIndex); + if ( pLedge ) + { + return pLedge->get_n_triangles(); + } + + return 0; +} + + +unsigned int CCollisionQuery::GetGameData( int convexIndex ) +{ + IVP_Compact_Ledge *pLedge = m_ledges.element_at( convexIndex ); + if ( pLedge ) + return pLedge->get_client_data(); + return 0; +} + + // Gets the triangle's verts to an array +void CCollisionQuery::GetTriangleVerts( int convexIndex, int triangleIndex, Vector *verts ) +{ + IVP_Compact_Ledge *pLedge = m_ledges.element_at( convexIndex ); + IVP_Compact_Triangle *pTriangle = Triangle( pLedge, triangleIndex ); + + int vertIndex = 0; + for ( int k = 2; k >= 0; k-- ) + { + const IVP_Compact_Edge *pEdge = pTriangle->get_edge( k ); + const IVP_U_Float_Point *pPoint = pEdge->get_start_point( pLedge ); + + Vector* pVec = verts + vertIndex; + ConvertPositionToHL( *pPoint, *pVec ); + vertIndex++; + } +} + +// UNDONE: This doesn't work!!! +void CCollisionQuery::SetTriangleVerts( int convexIndex, int triangleIndex, const Vector *verts ) +{ + IVP_Compact_Ledge *pLedge = m_ledges.element_at( convexIndex ); + Triangle( pLedge, triangleIndex ); +} + + +int CCollisionQuery::GetTriangleMaterialIndex( int convexIndex, int triangleIndex ) +{ + IVP_Compact_Ledge *pLedge = m_ledges.element_at( convexIndex ); + IVP_Compact_Triangle *pTriangle = Triangle( pLedge, triangleIndex ); + + return pTriangle->get_material_index(); +} + +void CCollisionQuery::SetTriangleMaterialIndex( int convexIndex, int triangleIndex, int index7bits ) +{ + IVP_Compact_Ledge *pLedge = m_ledges.element_at( convexIndex ); + IVP_Compact_Triangle *pTriangle = Triangle( pLedge, triangleIndex ); + + pTriangle->set_material_index( index7bits ); +} + +IVP_Compact_Triangle *CCollisionQuery::Triangle( IVP_Compact_Ledge *pLedge, int triangleIndex ) +{ + if ( !pLedge ) + return NULL; + + return pLedge->get_first_triangle() + triangleIndex; +} + + +#if 0 +void TestCubeVolume( void ) +{ + float volume = 0; + Vector verts[8]; + typedef struct + { + int a, b, c; + } triangle_t; + + triangle_t triangles[12] = + { + {0,1,3}, // front 0123 + {0,3,2}, + {4,5,1}, // top 4501 + {4,1,0}, + {2,3,7}, // bottom 2367 + {2,7,6}, + {1,5,7}, // right 1537 + {1,7,3}, + {4,0,2}, // left 4062 + {4,2,6}, + {5,4,6}, // back 5476 + {5,6,7} + }; + + int i = 0; + for ( int x = -1; x <= 1; x +=2 ) + for ( int y = -1; y <= 1; y +=2 ) + for ( int z = -1; z <= 1; z +=2 ) + { + verts[i][0] = x; + verts[i][1] = y; + verts[i][2] = z; + i++; + } + + + for ( i = 0; i < 12; i++ ) + { + triangle_t *pTri = triangles + i; + volume += TetrahedronVolume( verts[0], verts[pTri->a], verts[pTri->b], verts[pTri->c] ); + } + // should report a volume of 8. This is a cube that is 2 on a side + printf("Test volume %.4f\n", volume ); +} +#endif + + diff --git a/vphysics/physics_constraint.cpp b/vphysics/physics_constraint.cpp new file mode 100644 index 0000000..211cfef --- /dev/null +++ b/vphysics/physics_constraint.cpp @@ -0,0 +1,1842 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" +#include "vcollide_parse.h" + +#include "ivp_listener_object.hxx" +#include "vphysics/constraints.h" +#include "isaverestore.h" + +// HACKHACK: Mathlib defines this too! +#undef clamp +#undef max +#undef min + +// There's some constructor problems in the hk stuff... +// The classes inherit from other classes with private constructor +#pragma warning (disable : 4510 ) +#pragma warning (disable : 4610 ) + +// new havana constraint class +#include "hk_physics/physics.h" +#include "hk_physics/constraint/constraint.h" + +#include "hk_physics/constraint/breakable_constraint/breakable_constraint_bp.h" +#include "hk_physics/constraint/breakable_constraint/breakable_constraint.h" +#include "hk_physics/constraint/limited_ball_socket/limited_ball_socket_bp.h" +#include "hk_physics/constraint/limited_ball_socket/limited_ball_socket_constraint.h" +#include "hk_physics/constraint/fixed/fixed_bp.h" +#include "hk_physics/constraint/fixed/fixed_constraint.h" +#include "hk_physics/constraint/stiff_spring/stiff_spring_bp.h" +#include "hk_physics/constraint/stiff_spring/stiff_spring_constraint.h" + +#include "hk_physics/constraint/ball_socket/ball_socket_bp.h" +#include "hk_physics/constraint/ball_socket/ball_socket_constraint.h" + +#include "hk_physics/constraint/prismatic/prismatic_bp.h" +#include "hk_physics/constraint/prismatic/prismatic_constraint.h" + +#include "hk_physics/constraint/ragdoll/ragdoll_constraint.h" +#include "hk_physics/constraint/ragdoll/ragdoll_constraint_bp.h" +#include "hk_physics/constraint/ragdoll/ragdoll_constraint_bp_builder.h" + +#include "hk_physics/constraint/hinge/hinge_constraint.h" +#include "hk_physics/constraint/hinge/hinge_bp.h" +#include "hk_physics/constraint/hinge/hinge_bp_builder.h" + +#include "hk_physics/constraint/pulley/pulley_constraint.h" +#include "hk_physics/constraint/pulley/pulley_bp.h" + +#include "hk_physics/constraint/local_constraint_system/local_constraint_system.h" +#include "hk_physics/constraint/local_constraint_system/local_constraint_system_bp.h" + +#include "ivp_cache_object.hxx" +#include "ivp_template_constraint.hxx" +extern void qh_srand( int seed); +#include "qhull_user.hxx" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + + +const float UNBREAKABLE_BREAK_LIMIT = 1e12f; + +hk_Vector3 TransformHLWorldToHavanaLocal( const Vector &hlWorld, IVP_Real_Object *pObject ) +{ + IVP_U_Float_Point tmp; + IVP_U_Point pointOut; + ConvertPositionToIVP( hlWorld, tmp ); + + TransformIVPToLocal( tmp, pointOut, pObject, true ); + return hk_Vector3( pointOut.k[0], pointOut.k[1], pointOut.k[2] ); +} + +Vector TransformHavanaLocalToHLWorld( const hk_Vector3 &input, IVP_Real_Object *pObject, bool translate ) +{ + IVP_U_Float_Point ivpLocal( input.x, input.y, input.z ); + IVP_U_Float_Point ivpWorld; + + TransformLocalToIVP( ivpLocal, ivpWorld, pObject, translate ); + + Vector hlOut; + if ( translate ) + { + ConvertPositionToHL( ivpWorld, hlOut ); + } + else + { + ConvertDirectionToHL( ivpWorld, hlOut ); + } + return hlOut; +} + +inline hk_Vector3 vec( const IVP_U_Point &point ) +{ + hk_Vector3 tmp(point.k[0], point.k[1], point.k[2] ); + return tmp; +} + +// UNDONE: if vector were aligned we could simply cast these. +inline hk_Vector3 vec( const Vector &point ) +{ + hk_Vector3 tmp(point.x, point.y, point.z ); + return tmp; +} + +void ConvertHLLocalMatrixToHavanaLocal( const matrix3x4_t& hlMatrix, hk_Transform &out ) +{ + IVP_U_Matrix ivpMatrix; + ConvertMatrixToIVP( hlMatrix, ivpMatrix ); + ivpMatrix.get_4x4_column_major( (hk_real *)&out ); +} + +void set_4x4_column_major( IVP_U_Matrix &ivpMatrix, const float *in4x4 ) +{ + ivpMatrix.set_elem( 0, 0, in4x4[0] ); + ivpMatrix.set_elem( 1, 0, in4x4[1] ); + ivpMatrix.set_elem( 2, 0, in4x4[2] ); + + ivpMatrix.set_elem( 0, 1, in4x4[4] ); + ivpMatrix.set_elem( 1, 1, in4x4[5] ); + ivpMatrix.set_elem( 2, 1, in4x4[6] ); + + ivpMatrix.set_elem( 0, 2, in4x4[8] ); + ivpMatrix.set_elem( 1, 2, in4x4[9] ); + ivpMatrix.set_elem( 2, 2, in4x4[10] ); + + ivpMatrix.vv.k[0] = in4x4[12]; + ivpMatrix.vv.k[1] = in4x4[13]; + ivpMatrix.vv.k[2] = in4x4[14]; +} + +inline void ConvertPositionToIVP( const Vector &point, hk_Vector3 &out ) +{ + IVP_U_Float_Point ivp; + ConvertPositionToIVP( point, ivp ); + out = vec( ivp ); +} + +inline void ConvertPositionToHL( const hk_Vector3 &point, Vector& out ) +{ + float tmpY = IVP2HL(point.z); + out.z = -IVP2HL(point.y); + out.y = tmpY; + out.x = IVP2HL(point.x); +} + +inline void ConvertDirectionToHL( const hk_Vector3 &point, Vector& out ) +{ + float tmpY = point.z; + out.z = -point.y; + out.y = tmpY; + out.x = point.x; +} + +void ConvertHavanaLocalMatrixToHL( const hk_Transform &in, matrix3x4_t& hlMatrix, IVP_Real_Object *pObject ) +{ + IVP_U_Matrix ivpMatrix; + set_4x4_column_major( ivpMatrix, (const hk_real *)&in ); + ConvertMatrixToHL( ivpMatrix, hlMatrix ); +} + +static bool IsBreakableConstraint( const constraint_breakableparams_t &constraint ) +{ + return ( (constraint.forceLimit != 0 && constraint.forceLimit < UNBREAKABLE_BREAK_LIMIT) || + (constraint.torqueLimit != 0 && constraint.torqueLimit < UNBREAKABLE_BREAK_LIMIT) || + (constraint.bodyMassScale[0] != 1.0f && constraint.bodyMassScale[0] != 0.0f) || + (constraint.bodyMassScale[1] != 1.0f && constraint.bodyMassScale[1] != 0.0f) ) ? true : false; +} + +struct vphysics_save_cphysicsconstraintgroup_t : public constraint_groupparams_t +{ + bool isActive; + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_cphysicsconstraintgroup_t ) + DEFINE_FIELD( isActive, FIELD_BOOLEAN ), + DEFINE_FIELD( additionalIterations, FIELD_INTEGER ), + DEFINE_FIELD( minErrorTicks, FIELD_INTEGER ), + DEFINE_FIELD( errorTolerance, FIELD_FLOAT ), +END_DATADESC() + +// a little list that holds active groups so they can activate after +// the constraints are restored and added to the groups +static CUtlVector<CPhysicsConstraintGroup *> g_ConstraintGroupActivateList; + +class CPhysicsConstraintGroup: public IPhysicsConstraintGroup +{ +public: + hk_Local_Constraint_System *GetLCS() { return m_pLCS; } + + void WriteToTemplate( vphysics_save_cphysicsconstraintgroup_t &groupParams ) + { + hk_Local_Constraint_System_BP bp; + m_pLCS->write_to_blueprint( &bp ); + groupParams.additionalIterations = bp.m_n_iterations; + groupParams.isActive = bp.m_active; + groupParams.minErrorTicks = bp.m_minErrorTicks; + groupParams.errorTolerance = ConvertDistanceToHL(bp.m_errorTolerance); + } + +public: + CPhysicsConstraintGroup( IVP_Environment *pEnvironment, const constraint_groupparams_t &group ); + ~CPhysicsConstraintGroup(); + virtual void Activate(); + virtual bool IsInErrorState(); + virtual void ClearErrorState(); + void GetErrorParams( constraint_groupparams_t *pParams ); + void SetErrorParams( const constraint_groupparams_t ¶ms ); + void SolvePenetration( IPhysicsObject *pObj0, IPhysicsObject *pObj1 ); + +private: + hk_Local_Constraint_System *m_pLCS; +}; + +void CPhysicsConstraintGroup::Activate() +{ + if (m_pLCS) + { + m_pLCS->activate(); + } +} + +bool CPhysicsConstraintGroup::IsInErrorState() +{ + if (m_pLCS) + { + return m_pLCS->has_error(); + } + return false; +} + +void CPhysicsConstraintGroup::ClearErrorState() +{ + if (m_pLCS) + { + m_pLCS->clear_error(); + } +} + +void CPhysicsConstraintGroup::GetErrorParams( constraint_groupparams_t *pParams ) +{ + if ( !m_pLCS ) + return; + + vphysics_save_cphysicsconstraintgroup_t tmp; + WriteToTemplate( tmp ); + *pParams = tmp; +} + + +void CPhysicsConstraintGroup::SetErrorParams( const constraint_groupparams_t ¶ms ) +{ + if ( !m_pLCS ) + return; + + m_pLCS->set_error_ticks( params.minErrorTicks ); + m_pLCS->set_error_tolerance( ConvertDistanceToIVP(params.errorTolerance) ); +} + +void CPhysicsConstraintGroup::SolvePenetration( IPhysicsObject *pObj0, IPhysicsObject *pObj1 ) +{ + if ( m_pLCS && pObj0 && pObj1 ) + { + CPhysicsObject *pPhys0 = static_cast<CPhysicsObject *>(pObj0); + CPhysicsObject *pPhys1 = static_cast<CPhysicsObject *>(pObj1); + m_pLCS->solve_penetration(pPhys0->GetObject(), pPhys1->GetObject()); + } +} + + +CPhysicsConstraintGroup::~CPhysicsConstraintGroup() +{ + delete m_pLCS; +} + + +CPhysicsConstraintGroup::CPhysicsConstraintGroup( IVP_Environment *pEnvironment, const constraint_groupparams_t &group ) +{ + hk_Local_Constraint_System_BP cs_bp; + cs_bp.m_n_iterations = group.additionalIterations; + cs_bp.m_minErrorTicks = group.minErrorTicks; + cs_bp.m_errorTolerance = ConvertDistanceToIVP(group.errorTolerance); + m_pLCS = new hk_Local_Constraint_System( static_cast<hk_Environment *>(pEnvironment), &cs_bp ); + m_pLCS->set_client_data( (void *)this ); +} + +enum vphysics_save_constrainttypes_t +{ + CONSTRAINT_UNKNOWN = 0, + CONSTRAINT_RAGDOLL, + CONSTRAINT_HINGE, + CONSTRAINT_FIXED, + CONSTRAINT_BALLSOCKET, + CONSTRAINT_SLIDING, + CONSTRAINT_PULLEY, + CONSTRAINT_LENGTH, +}; + +struct vphysics_save_cphysicsconstraint_t +{ + int constraintType; + CPhysicsConstraintGroup *pGroup; + CPhysicsObject *pObjReference; + CPhysicsObject *pObjAttached; + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_cphysicsconstraint_t ) + DEFINE_FIELD( constraintType, FIELD_INTEGER ), + DEFINE_VPHYSPTR( pGroup ), + DEFINE_VPHYSPTR( pObjReference ), + DEFINE_VPHYSPTR( pObjAttached ), +END_DATADESC() + +struct vphysics_save_constraintbreakable_t : public constraint_breakableparams_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constraintbreakable_t ) + DEFINE_FIELD( strength, FIELD_FLOAT ), + DEFINE_FIELD( forceLimit, FIELD_FLOAT ), + DEFINE_FIELD( torqueLimit, FIELD_FLOAT ), + DEFINE_AUTO_ARRAY( bodyMassScale, FIELD_FLOAT ), + DEFINE_FIELD( isActive, FIELD_BOOLEAN ), +END_DATADESC() + +struct vphysics_save_constraintaxislimit_t : public constraint_axislimit_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constraintaxislimit_t ) + DEFINE_FIELD( minRotation, FIELD_FLOAT ), + DEFINE_FIELD( maxRotation, FIELD_FLOAT ), + DEFINE_FIELD( angularVelocity, FIELD_FLOAT ), + DEFINE_FIELD( torque, FIELD_FLOAT ), +END_DATADESC() + +struct vphysics_save_constraintfixed_t : public constraint_fixedparams_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constraintfixed_t ) + DEFINE_AUTO_ARRAY2D( attachedRefXform, FIELD_FLOAT ), + DEFINE_EMBEDDED_OVERRIDE( constraint, vphysics_save_constraintbreakable_t ), +END_DATADESC() + +struct vphysics_save_constrainthinge_t : public constraint_hingeparams_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constrainthinge_t ) + DEFINE_FIELD( worldPosition, FIELD_POSITION_VECTOR ), + DEFINE_FIELD( worldAxisDirection, FIELD_VECTOR ), + DEFINE_EMBEDDED_OVERRIDE( constraint, vphysics_save_constraintbreakable_t ), + DEFINE_EMBEDDED_OVERRIDE( hingeAxis, vphysics_save_constraintaxislimit_t ), +END_DATADESC() + +struct vphysics_save_constraintsliding_t : public constraint_slidingparams_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constraintsliding_t ) + DEFINE_AUTO_ARRAY2D( attachedRefXform, FIELD_FLOAT ), + DEFINE_FIELD( slideAxisRef, FIELD_VECTOR ), + DEFINE_FIELD( limitMin, FIELD_FLOAT ), + DEFINE_FIELD( limitMax, FIELD_FLOAT ), + DEFINE_FIELD( friction, FIELD_FLOAT ), + DEFINE_FIELD( velocity, FIELD_FLOAT ), + DEFINE_EMBEDDED_OVERRIDE( constraint, vphysics_save_constraintbreakable_t ), +END_DATADESC() + +struct vphysics_save_constraintpulley_t : public constraint_pulleyparams_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constraintpulley_t ) + DEFINE_AUTO_ARRAY( pulleyPosition, FIELD_POSITION_VECTOR ), + DEFINE_AUTO_ARRAY( objectPosition, FIELD_VECTOR ), + DEFINE_FIELD( totalLength, FIELD_FLOAT ), + DEFINE_FIELD( gearRatio, FIELD_FLOAT ), + DEFINE_FIELD( isRigid, FIELD_BOOLEAN ), + DEFINE_EMBEDDED_OVERRIDE( constraint, vphysics_save_constraintbreakable_t ), +END_DATADESC() + +struct vphysics_save_constraintlength_t : public constraint_lengthparams_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constraintlength_t ) + DEFINE_AUTO_ARRAY( objectPosition, FIELD_VECTOR ), + DEFINE_FIELD( totalLength, FIELD_FLOAT ), + DEFINE_FIELD( minLength, FIELD_FLOAT ), + DEFINE_EMBEDDED_OVERRIDE( constraint, vphysics_save_constraintbreakable_t ), +END_DATADESC() + +struct vphysics_save_constraintballsocket_t : public constraint_ballsocketparams_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constraintballsocket_t ) + DEFINE_AUTO_ARRAY( constraintPosition, FIELD_VECTOR ), + DEFINE_EMBEDDED_OVERRIDE( constraint, vphysics_save_constraintbreakable_t ), +END_DATADESC() + +struct vphysics_save_constraintragdoll_t : public constraint_ragdollparams_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_constraintragdoll_t ) + DEFINE_EMBEDDED_OVERRIDE( constraint, vphysics_save_constraintbreakable_t ), + DEFINE_AUTO_ARRAY2D( constraintToReference, FIELD_FLOAT ), + DEFINE_AUTO_ARRAY2D( constraintToAttached, FIELD_FLOAT ), + DEFINE_EMBEDDED_OVERRIDE( axes[0], vphysics_save_constraintaxislimit_t ), + DEFINE_EMBEDDED_OVERRIDE( axes[1], vphysics_save_constraintaxislimit_t ), + DEFINE_EMBEDDED_OVERRIDE( axes[2], vphysics_save_constraintaxislimit_t ), + DEFINE_FIELD( onlyAngularLimits, FIELD_BOOLEAN ), + DEFINE_FIELD( isActive, FIELD_BOOLEAN ), + DEFINE_FIELD( useClockwiseRotations, FIELD_BOOLEAN ), +END_DATADESC() + +struct vphysics_save_constraint_t +{ + vphysics_save_constraintfixed_t fixed; + vphysics_save_constrainthinge_t hinge; + vphysics_save_constraintsliding_t sliding; + vphysics_save_constraintpulley_t pulley; + vphysics_save_constraintlength_t length; + vphysics_save_constraintballsocket_t ballsocket; + vphysics_save_constraintragdoll_t ragdoll; +}; + +// UNDONE: May need to change interface to specify limits before construction +// UNDONE: Refactor constraints to contain a separate object for the various constraint types? +class CPhysicsConstraint: public IPhysicsConstraint, public IVP_Listener_Object +{ +public: + CPhysicsConstraint( CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject ); + ~CPhysicsConstraint( void ); + + // init as ragdoll constraint + void InitRagdoll( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_ragdollparams_t &ragdoll ); + // init as hinge + void InitHinge( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_limitedhingeparams_t &hinge ); + // init as fixed (BUGBUG: This is broken) + void InitFixed( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_fixedparams_t &fixed ); + // init as ballsocket + void InitBallsocket( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_ballsocketparams_t &ballsocket ); + // init as sliding constraint + void InitSliding( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_slidingparams_t &sliding ); + // init as pulley + void InitPulley( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_pulleyparams_t &pulley ); + // init as stiff spring / length constraint + void InitLength( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_lengthparams_t &length ); + + void WriteToTemplate( vphysics_save_cphysicsconstraint_t &header, vphysics_save_constraint_t &constraintTemplate ) const; + + void WriteRagdoll( constraint_ragdollparams_t &ragdoll ) const; + void WriteHinge( constraint_hingeparams_t &hinge ) const; + void WriteFixed( constraint_fixedparams_t &fixed ) const; + void WriteSliding( constraint_slidingparams_t &sliding ) const; + void WriteBallsocket( constraint_ballsocketparams_t &ballsocket ) const; + void WritePulley( constraint_pulleyparams_t &pulley ) const; + void WriteLength( constraint_lengthparams_t &length ) const; + CPhysicsConstraintGroup *GetConstraintGroup() const; + + hk_Constraint *CreateBreakableConstraint( hk_Constraint *pRealConstraint, hk_Local_Constraint_System *pLcs, const constraint_breakableparams_t &constraint ) + { + m_isBreakable = true; + hk_Breakable_Constraint_BP bp; + bp.m_real_constraint = pRealConstraint; + float forceLimit = ConvertDistanceToIVP( constraint.forceLimit ); + bp.m_linear_strength = forceLimit > 0 ? forceLimit : UNBREAKABLE_BREAK_LIMIT; + bp.m_angular_strength = constraint.torqueLimit > 0 ? DEG2RAD(constraint.torqueLimit) : UNBREAKABLE_BREAK_LIMIT; + bp.m_bodyMassScale[0] = constraint.bodyMassScale[0] > 0 ? constraint.bodyMassScale[0] : 1.0f; + bp.m_bodyMassScale[1] = constraint.bodyMassScale[1] > 0 ? constraint.bodyMassScale[1] : 1.0f;; + return new hk_Breakable_Constraint( pLcs, &bp ); + } + void ReadBreakableConstraint( constraint_breakableparams_t ¶ms ) const; + + hk_Constraint *GetRealConstraint() const + { + if ( m_isBreakable ) + { + hk_Breakable_Constraint_BP bp; + ((hk_Breakable_Constraint *)m_HkConstraint)->write_to_blueprint( &bp ); + return bp.m_real_constraint; + } + return m_HkConstraint; + } + + void Activate( void ); + void Deactivate( void ); + void SetupRagdollAxis( int axis, const constraint_axislimit_t &axisData, hk_Limited_Ball_Socket_BP *ballsocketBP ); + // UNDONE: Implement includeStatic for havana + + void SetGameData( void *gameData ) { m_pGameData = gameData; } + void *GetGameData( void ) const { return m_pGameData; } + IPhysicsObject *GetReferenceObject( void ) const; + IPhysicsObject *GetAttachedObject( void ) const; + + void SetLinearMotor( float speed, float maxForce ); + void SetAngularMotor( float rotSpeed, float maxAngularImpulse ); + void UpdateRagdollTransforms( const matrix3x4_t &constraintToReference, const matrix3x4_t &constraintToAttached ); + bool GetConstraintTransform( matrix3x4_t *pConstraintToReference, matrix3x4_t *pConstraintToAttached ) const; + bool GetConstraintParams( constraint_breakableparams_t *pParams ) const; + void OutputDebugInfo() + { + hk_Local_Constraint_System *pLCS = m_HkLCS; + if ( m_HkConstraint ) + { + pLCS = m_HkConstraint->get_constraint_system(); + } + if ( pLCS ) + { + int count = 0; + hk_Array<hk_Constraint *> list; + pLCS->get_constraints_in_system( list ); + Msg("System of %d constraints\n", list.length()); + for ( hk_Array<hk_Constraint*>::iterator i = list.start(); list.is_valid(i); i = list.next(i) ) + { + hk_Constraint *pConstraint = list.get_element(i); + Msg("\tConstraint %d) %s\n", count, pConstraint->get_constraint_type() ); + count++; + } + } + } + + void DetachListener(); + // Object listener + virtual void event_object_deleted( IVP_Event_Object *); + virtual void event_object_created( IVP_Event_Object *) {} + virtual void event_object_revived( IVP_Event_Object *) {} + virtual void event_object_frozen ( IVP_Event_Object *) {} + +private: + CPhysicsObject *m_pObjReference; + CPhysicsObject *m_pObjAttached; + + // havana constraints + hk_Constraint *m_HkConstraint; + hk_Local_Constraint_System *m_HkLCS; + void *m_pGameData; + // these are used to crack the abstract pointers on save/load + short m_constraintType; + short m_isBreakable; +}; + +CPhysicsConstraint::CPhysicsConstraint( CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject ) +{ + m_pGameData = NULL; + m_HkConstraint = NULL; + m_HkLCS = NULL; + m_constraintType = CONSTRAINT_UNKNOWN; + m_isBreakable = false; + + if ( pReferenceObject && pAttachedObject ) + { + m_pObjReference = (CPhysicsObject *)pReferenceObject; + m_pObjAttached = (CPhysicsObject *)pAttachedObject; + if ( !(m_pObjReference->CallbackFlags() & CALLBACK_NEVER_DELETED) ) + { + m_pObjReference->GetObject()->add_listener_object( this ); + } + if ( !(m_pObjAttached->CallbackFlags() & CALLBACK_NEVER_DELETED) ) + { + m_pObjAttached->GetObject()->add_listener_object( this ); + } + } + else + { + m_pObjReference = NULL; + m_pObjAttached = NULL; + } +} + +// Check to see if this is a single degree of freedom joint, if so, convert to a hinge +static bool ConvertRagdollToHinge( constraint_limitedhingeparams_t *pHingeOut, const constraint_ragdollparams_t &ragdoll, IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject ) +{ + int nDOF = 0; + int dofIndex = 0; + for ( int i = 0; i < 3; i++ ) + { + if ( ragdoll.axes[i].minRotation != ragdoll.axes[i].maxRotation ) + { + dofIndex = i; + nDOF++; + } + } + + // found multiple degrees of freedom + if ( nDOF != 1 ) + return false; + + // convert params to hinge + pHingeOut->Defaults(); + pHingeOut->constraint = ragdoll.constraint; + + // get the hinge axis in world space + matrix3x4_t refToWorld, constraintToWorld; + pReferenceObject->GetPositionMatrix( &refToWorld ); + ConcatTransforms( refToWorld, ragdoll.constraintToReference, constraintToWorld ); + // many ragdoll constraints don't set this and the ragdoll solver ignores it + // force it to the default + pHingeOut->constraint.strength = 1.0f; + MatrixGetColumn( constraintToWorld, 3, &pHingeOut->worldPosition ); + MatrixGetColumn( constraintToWorld, dofIndex, &pHingeOut->worldAxisDirection ); + pHingeOut->referencePerpAxisDirection.Init(); + pHingeOut->referencePerpAxisDirection[(dofIndex+1)%3] = 1; + + Vector perpCS; + VectorIRotate( pHingeOut->referencePerpAxisDirection, ragdoll.constraintToReference, perpCS ); + VectorRotate( perpCS, ragdoll.constraintToAttached, pHingeOut->attachedPerpAxisDirection ); + + pHingeOut->hingeAxis = ragdoll.axes[dofIndex]; + + // Funky math to insure that the friction is preserved after the math that the hinge code uses. + pHingeOut->hingeAxis.torque = RAD2DEG( pHingeOut->hingeAxis.torque * pReferenceObject->GetMass() ); + + // need to flip the limits, just flip the axis instead + if ( !ragdoll.useClockwiseRotations ) + { + float tmp = pHingeOut->hingeAxis.minRotation; + pHingeOut->hingeAxis.minRotation = -pHingeOut->hingeAxis.maxRotation; + pHingeOut->hingeAxis.maxRotation = -tmp; + } + + return true; +} + +// ragdoll constraint +void CPhysicsConstraint::InitRagdoll( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_ragdollparams_t &ragdoll ) +{ + // UNDONE: If this is a hinge parameterized using the ragdoll params, use a hinge instead! + constraint_limitedhingeparams_t hinge; + if ( ConvertRagdollToHinge( &hinge, ragdoll, m_pObjReference, m_pObjAttached ) ) + { + InitHinge( pEnvironment, constraint_group, hinge ); + return; + } + + m_constraintType = CONSTRAINT_RAGDOLL; + + hk_Rigid_Body *ref = (hk_Rigid_Body*)m_pObjReference->GetObject(); + hk_Rigid_Body *att = (hk_Rigid_Body*)m_pObjAttached->GetObject(); + + hk_Limited_Ball_Socket_BP ballsocketBP; + ConvertHLLocalMatrixToHavanaLocal( ragdoll.constraintToReference, ballsocketBP.m_transform_os_ks[0] ); + ConvertHLLocalMatrixToHavanaLocal( ragdoll.constraintToAttached, ballsocketBP.m_transform_os_ks[1] ); + + bool breakable = IsBreakableConstraint( ragdoll.constraint ); + + int i; + + // BUGBUG: Handle incorrect clockwise rotations here + for ( i = 0; i < 3; i++ ) + { + SetupRagdollAxis( i, ragdoll.axes[i], &ballsocketBP ); + } + ballsocketBP.m_constrainTranslation = ragdoll.onlyAngularLimits ? false : true; + + // swap the input limits if they are clockwise (angles are counter-clockwise) + if ( ragdoll.useClockwiseRotations ) + { + for ( i = 0; i < 3; i++ ) + { + float tmp = ballsocketBP.m_angular_limits[i].m_min; + ballsocketBP.m_angular_limits[i].m_min = -ballsocketBP.m_angular_limits[i].m_max; + ballsocketBP.m_angular_limits[i].m_max = -tmp; + } + } + + hk_Ragdoll_Constraint_BP_Builder r_builder; + r_builder.initialize_from_limited_ball_socket_bp( &ballsocketBP, ref, att ); + hk_Ragdoll_Constraint_BP *bp = (hk_Ragdoll_Constraint_BP *)r_builder.get_blueprint(); // get non const bp + + int revAxisMapHK[3]; + revAxisMapHK[bp->m_axisMap[0]] = 0; + revAxisMapHK[bp->m_axisMap[1]] = 1; + revAxisMapHK[bp->m_axisMap[2]] = 2; + for ( i = 0; i < 3; i++ ) + { + // remap HL axis to IVP axis + int ivpAxis = ConvertCoordinateAxisToIVP( i ); + + // initialize_from_limited_ball_socket_bp remapped the axes too! So remap again. + int hkAxis = revAxisMapHK[ivpAxis]; + + const constraint_axislimit_t &axisData = ragdoll.axes[i]; + bp->m_limits[hkAxis].set_motor( DEG2RAD(axisData.angularVelocity), axisData.torque * m_pObjReference->GetMass() ); + bp->m_tau = 1.0f; + } + + + hk_Local_Constraint_System *lcs = constraint_group ? constraint_group->GetLCS() : NULL; + hk_Environment *hkEnvironment = static_cast<hk_Environment *>(pEnvironment); + if ( !lcs ) + { + hk_Local_Constraint_System_BP bp; + lcs = new hk_Local_Constraint_System( hkEnvironment, &bp ); + m_HkLCS = lcs; + } + + if ( breakable ) + { + hk_Ragdoll_Constraint *pConstraint = new hk_Ragdoll_Constraint( hkEnvironment, bp, ref, att); + m_HkConstraint = CreateBreakableConstraint( pConstraint, lcs, ragdoll.constraint ); + } + else + { + m_HkConstraint = new hk_Ragdoll_Constraint( lcs, bp, ref, att); + } + + if ( m_HkLCS && ragdoll.isActive ) + { + m_HkLCS->activate(); + } + m_HkConstraint->set_client_data( (void *)this ); +} + +// hinge constraint +void CPhysicsConstraint::InitHinge( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_limitedhingeparams_t &hinge ) +{ + m_constraintType = CONSTRAINT_HINGE; + hk_Environment *hkEnvironment = static_cast<hk_Environment *>(pEnvironment); + + bool breakable = IsBreakableConstraint( hinge.constraint ); + + hk_Hinge_BP_Builder builder; + + IVP_U_Point axisIVP_ws, axisPerpIVP_os, axisStartIVP_ws, axisStartIVP_os; + + ConvertDirectionToIVP( hinge.worldAxisDirection, axisIVP_ws ); + builder.set_axis_ws( (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject(), vec(axisIVP_ws) ); + builder.set_position_os( 0, TransformHLWorldToHavanaLocal( hinge.worldPosition, m_pObjReference->GetObject() ) ); + builder.set_position_os( 1, TransformHLWorldToHavanaLocal( hinge.worldPosition, m_pObjAttached->GetObject() ) ); + + ConvertDirectionToIVP( hinge.referencePerpAxisDirection, axisPerpIVP_os ); + builder.set_axis_perp_os( 0, vec(axisPerpIVP_os) ); + ConvertDirectionToIVP( hinge.attachedPerpAxisDirection, axisPerpIVP_os ); + builder.set_axis_perp_os( 1, vec(axisPerpIVP_os) ); + + builder.set_tau( hinge.constraint.strength ); + // torque is an impulse radians/sec * inertia + if ( hinge.hingeAxis.torque != 0 ) + { + builder.set_angular_motor( DEG2RAD(hinge.hingeAxis.angularVelocity), DEG2RAD(hinge.hingeAxis.torque) ); + } + if ( hinge.hingeAxis.minRotation != hinge.hingeAxis.maxRotation ) + { + builder.set_angular_limits( DEG2RAD(hinge.hingeAxis.minRotation), DEG2RAD(hinge.hingeAxis.maxRotation) ); + } + hk_Local_Constraint_System *lcs = constraint_group ? constraint_group->GetLCS() : NULL; + if ( !lcs ) + { + hk_Local_Constraint_System_BP bp; + lcs = new hk_Local_Constraint_System( hkEnvironment, &bp ); + m_HkLCS = lcs; + } + + if ( breakable ) + { + hk_Hinge_Constraint *pHinge = new hk_Hinge_Constraint( hkEnvironment, builder.get_blueprint(), (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + m_HkConstraint = CreateBreakableConstraint( pHinge, lcs, hinge.constraint ); + } + else + { + m_HkConstraint = new hk_Hinge_Constraint( lcs, builder.get_blueprint(), (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + } + if ( m_HkLCS && hinge.constraint.isActive ) + { + m_HkLCS->activate(); + } + m_HkConstraint->set_client_data( (void *)this ); +} + + +// fixed constraint +void CPhysicsConstraint::InitFixed( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_fixedparams_t &fixed ) +{ + m_constraintType = CONSTRAINT_FIXED; + hk_Environment *hkEnvironment = static_cast<hk_Environment *>(pEnvironment); + + bool breakable = IsBreakableConstraint( fixed.constraint ); + + hk_Fixed_BP fixed_bp; + ConvertHLLocalMatrixToHavanaLocal( fixed.attachedRefXform, fixed_bp.m_transform_os_ks ); + + fixed_bp.m_tau = fixed.constraint.strength; + + hk_Local_Constraint_System *lcs = constraint_group ? constraint_group->GetLCS() : NULL; + if ( !lcs ) + { + hk_Local_Constraint_System_BP bp; + lcs = new hk_Local_Constraint_System( hkEnvironment, &bp ); + m_HkLCS = lcs; + } + + if ( breakable ) + { + hk_Constraint *pFixed = new hk_Fixed_Constraint( hkEnvironment, &fixed_bp, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + + m_HkConstraint = CreateBreakableConstraint( pFixed, lcs, fixed.constraint ); + } + else + { + m_HkConstraint = new hk_Fixed_Constraint( lcs, &fixed_bp, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + } + + if ( m_HkLCS && fixed.constraint.isActive ) + { + m_HkLCS->activate(); + } + m_HkConstraint->set_client_data( (void *)this ); +} + +void CPhysicsConstraint::InitBallsocket( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_ballsocketparams_t &ballsocket ) +{ + m_constraintType = CONSTRAINT_BALLSOCKET; + + hk_Environment *hkEnvironment = static_cast<hk_Environment *>(pEnvironment); + + bool breakable = IsBreakableConstraint( ballsocket.constraint ); + + hk_Ball_Socket_BP builder; + + for ( int i = 0; i < 2; i++ ) + { + hk_Vector3 hkConstraintLocal; + ConvertPositionToIVP( ballsocket.constraintPosition[i], hkConstraintLocal ); + builder.set_position_os( i, hkConstraintLocal ); + } + + builder.m_strength = ballsocket.constraint.strength; + hk_Local_Constraint_System *lcs = constraint_group ? constraint_group->GetLCS() : NULL; + if ( !lcs ) + { + hk_Local_Constraint_System_BP bp; + lcs = new hk_Local_Constraint_System( hkEnvironment, &bp ); + m_HkLCS = lcs; + } + + if ( breakable ) + { + hk_Ball_Socket_Constraint *pConstraint = new hk_Ball_Socket_Constraint( hkEnvironment, &builder, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + m_HkConstraint = CreateBreakableConstraint( pConstraint, lcs, ballsocket.constraint ); + } + else + { + m_HkConstraint = new hk_Ball_Socket_Constraint( lcs, &builder, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + } + + if ( m_HkLCS && ballsocket.constraint.isActive ) + { + m_HkLCS->activate(); + } + m_HkConstraint->set_client_data( (void *)this ); +} + +void CPhysicsConstraint::InitSliding( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_slidingparams_t &sliding ) +{ + m_constraintType = CONSTRAINT_SLIDING; + hk_Environment *hkEnvironment = static_cast<hk_Environment *>(pEnvironment); + + bool breakable = IsBreakableConstraint( sliding.constraint ); + + hk_Prismatic_BP prismatic_bp; + hk_Transform t; + ConvertHLLocalMatrixToHavanaLocal( sliding.attachedRefXform, t ); + prismatic_bp.m_transform_Ros_Aos.m_translation = t.get_translation(); + prismatic_bp.m_transform_Ros_Aos.m_rotation.set( t ); + + IVP_U_Float_Point refAxisDir; + ConvertDirectionToIVP( sliding.slideAxisRef, refAxisDir ); + prismatic_bp.m_axis_Ros = vec(refAxisDir); + prismatic_bp.m_tau = sliding.constraint.strength; + + hk_Constraint_Limit_BP bp; + + if ( sliding.limitMin != sliding.limitMax ) + { + bp.set_limits( ConvertDistanceToIVP(sliding.limitMin), ConvertDistanceToIVP(sliding.limitMax) ); + } + if ( sliding.friction ) + { + if ( sliding.velocity ) + { + bp.set_motor( ConvertDistanceToIVP(sliding.velocity), ConvertDistanceToIVP(sliding.friction) ); + } + else + { + bp.set_friction( ConvertDistanceToIVP(sliding.friction) ); + } + } + prismatic_bp.m_limit.init_limit( bp, 1.0 ); + + hk_Local_Constraint_System *lcs = constraint_group ? constraint_group->GetLCS() : NULL; + if ( !lcs ) + { + hk_Local_Constraint_System_BP bp; + lcs = new hk_Local_Constraint_System( hkEnvironment, &bp ); + m_HkLCS = lcs; + } + + if ( breakable ) + { + hk_Constraint *pFixed = new hk_Prismatic_Constraint( hkEnvironment, &prismatic_bp, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + + m_HkConstraint = CreateBreakableConstraint( pFixed, lcs, sliding.constraint ); + } + else + { + m_HkConstraint = new hk_Prismatic_Constraint( lcs, &prismatic_bp, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + } + + if ( m_HkLCS && sliding.constraint.isActive ) + { + m_HkLCS->activate(); + } + m_HkConstraint->set_client_data( (void *)this ); +} + +void CPhysicsConstraint::InitPulley( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_pulleyparams_t &pulley ) +{ + m_constraintType = CONSTRAINT_PULLEY; + + hk_Environment *hkEnvironment = static_cast<hk_Environment *>(pEnvironment); + + bool breakable = IsBreakableConstraint( pulley.constraint ); + + hk_Pulley_BP pulley_bp; + pulley_bp.m_tau = pulley.constraint.strength; + //pulley_bp.m_strength = pulley.constraint.strength; + pulley_bp.m_gearing = pulley.gearRatio; + pulley_bp.m_is_rigid = pulley.isRigid; + + // Get the current length of rope + pulley_bp.m_length = ConvertDistanceToIVP( pulley.totalLength ); + + // set the anchor positions in object space + ConvertPositionToIVP( pulley.objectPosition[0], pulley_bp.m_translation_os_ks[0] ); + ConvertPositionToIVP( pulley.objectPosition[1], pulley_bp.m_translation_os_ks[1] ); + + // set the pully positions in world space + ConvertPositionToIVP( pulley.pulleyPosition[0], pulley_bp.m_worldspace_point[0] ); + ConvertPositionToIVP( pulley.pulleyPosition[1], pulley_bp.m_worldspace_point[1] ); + + hk_Local_Constraint_System *lcs = constraint_group ? constraint_group->GetLCS() : NULL; + if ( !lcs ) + { + hk_Local_Constraint_System_BP bp; + lcs = new hk_Local_Constraint_System( hkEnvironment, &bp ); + m_HkLCS = lcs; + } + + if ( breakable ) + { + hk_Constraint *pPulley = new hk_Pulley_Constraint( hkEnvironment, &pulley_bp, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + + m_HkConstraint = CreateBreakableConstraint( pPulley, lcs, pulley.constraint ); + } + else + { + m_HkConstraint = new hk_Pulley_Constraint( lcs, &pulley_bp, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + } + + if ( m_HkLCS && pulley.constraint.isActive ) + { + m_HkLCS->activate(); + } + m_HkConstraint->set_client_data( (void *)this ); +} + + +void CPhysicsConstraint::InitLength( IVP_Environment *pEnvironment, CPhysicsConstraintGroup *constraint_group, const constraint_lengthparams_t &length ) +{ + m_constraintType = CONSTRAINT_LENGTH; + + hk_Environment *hkEnvironment = static_cast<hk_Environment *>(pEnvironment); + + bool breakable = IsBreakableConstraint( length.constraint ); + + hk_Stiff_Spring_BP stiff_bp; + stiff_bp.m_tau = length.constraint.strength; + //stiff_bp.m_strength = length.constraint.strength; + + // Get the current length of rope + stiff_bp.m_length = ConvertDistanceToIVP( length.totalLength ); + stiff_bp.m_min_length = ConvertDistanceToIVP( length.minLength ); + + // set the anchor positions in object space + ConvertPositionToIVP( length.objectPosition[0], stiff_bp.m_translation_os_ks[0] ); + ConvertPositionToIVP( length.objectPosition[1], stiff_bp.m_translation_os_ks[1] ); + + hk_Local_Constraint_System *lcs = constraint_group ? constraint_group->GetLCS() : NULL; + if ( !lcs ) + { + hk_Local_Constraint_System_BP bp; + lcs = new hk_Local_Constraint_System( hkEnvironment, &bp ); + m_HkLCS = lcs; + } + + if ( breakable ) + { + hk_Constraint *pLength = new hk_Stiff_Spring_Constraint( hkEnvironment, &stiff_bp, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + + m_HkConstraint = CreateBreakableConstraint( pLength, lcs, length.constraint ); + } + else + { + m_HkConstraint = new hk_Stiff_Spring_Constraint( lcs, &stiff_bp, (hk_Rigid_Body*)m_pObjReference->GetObject(), (hk_Rigid_Body*)m_pObjAttached->GetObject() ); + } + + if ( m_HkLCS && length.constraint.isActive ) + { + m_HkLCS->activate(); + } + m_HkConstraint->set_client_data( (void *)this ); +} + +// Serialization: Write out a description for this constraint +void CPhysicsConstraint::WriteToTemplate( vphysics_save_cphysicsconstraint_t &header, vphysics_save_constraint_t &constraintTemplate ) const +{ + header.constraintType = m_constraintType; + + // this constraint is inert due to one of it's objects getting deleted + if ( !m_HkConstraint ) + return; + + header.pGroup = GetConstraintGroup(); + + header.pObjReference = m_pObjReference; + header.pObjAttached = m_pObjAttached; + + switch( header.constraintType ) + { + case CONSTRAINT_UNKNOWN: + Assert(0); + break; + case CONSTRAINT_HINGE: + WriteHinge( constraintTemplate.hinge ); + break; + case CONSTRAINT_FIXED: + WriteFixed( constraintTemplate.fixed ); + break; + case CONSTRAINT_SLIDING: + WriteSliding( constraintTemplate.sliding ); + break; + case CONSTRAINT_PULLEY: + WritePulley( constraintTemplate.pulley ); + break; + case CONSTRAINT_LENGTH: + WriteLength( constraintTemplate.length ); + break; + case CONSTRAINT_BALLSOCKET: + WriteBallsocket( constraintTemplate.ballsocket ); + break; + case CONSTRAINT_RAGDOLL: + WriteRagdoll( constraintTemplate.ragdoll ); + break; + } +} + +void CPhysicsConstraint::SetLinearMotor( float speed, float maxLinearImpulse ) +{ + if ( m_constraintType != CONSTRAINT_SLIDING ) + return; + + hk_Prismatic_Constraint *pConstraint = (hk_Prismatic_Constraint *)GetRealConstraint(); + pConstraint->set_motor( ConvertDistanceToIVP( speed ), ConvertDistanceToIVP( maxLinearImpulse ) ); +} + +void CPhysicsConstraint::SetAngularMotor( float rotSpeed, float maxAngularImpulse ) +{ + if ( m_constraintType == CONSTRAINT_RAGDOLL && rotSpeed == 0 ) + { + hk_Ragdoll_Constraint *pConstraint = (hk_Ragdoll_Constraint *)GetRealConstraint(); + pConstraint->update_friction( ConvertAngleToIVP( maxAngularImpulse ) ); + } + else if ( m_constraintType == CONSTRAINT_HINGE ) + { + hk_Hinge_Constraint *pConstraint = (hk_Hinge_Constraint *)GetRealConstraint(); + pConstraint->set_motor( ConvertAngleToIVP( rotSpeed ), ConvertAngleToIVP( fabs(maxAngularImpulse) ) ); + } +} + +void CPhysicsConstraint::UpdateRagdollTransforms( const matrix3x4_t &constraintToReference, const matrix3x4_t &constraintToAttached ) +{ + if ( m_constraintType != CONSTRAINT_RAGDOLL ) + return; + + hk_Transform os_ks_0, os_ks_1; + + ConvertHLLocalMatrixToHavanaLocal( constraintToReference, os_ks_0 ); + ConvertHLLocalMatrixToHavanaLocal( constraintToAttached, os_ks_1 ); + + hk_Ragdoll_Constraint *pConstraint = (hk_Ragdoll_Constraint *)GetRealConstraint(); + pConstraint->update_transforms( os_ks_0, os_ks_1 ); +} + +bool CPhysicsConstraint::GetConstraintTransform( matrix3x4_t *pConstraintToReference, matrix3x4_t *pConstraintToAttached ) const +{ + if ( m_constraintType == CONSTRAINT_RAGDOLL ) + { + hk_Ragdoll_Constraint *pConstraint = (hk_Ragdoll_Constraint *)GetRealConstraint(); + if ( pConstraintToReference ) + { + ConvertHavanaLocalMatrixToHL( pConstraint->get_transform(0), *pConstraintToReference, NULL ); + } + if ( pConstraintToAttached ) + { + ConvertHavanaLocalMatrixToHL( pConstraint->get_transform(1), *pConstraintToAttached, NULL ); + } + return true; + } + else if ( m_constraintType == CONSTRAINT_BALLSOCKET ) + { + hk_Ball_Socket_Constraint *pConstraint = (hk_Ball_Socket_Constraint *)GetRealConstraint(); + Vector pos; + if ( pConstraintToReference ) + { + ConvertPositionToHL( pConstraint->get_transform(0), pos ); + AngleMatrix( vec3_angle, pos, *pConstraintToReference ); + } + if ( pConstraintToAttached ) + { + ConvertPositionToHL( pConstraint->get_transform(1), pos ); + AngleMatrix( vec3_angle, pos, *pConstraintToAttached ); + } + return true; + } + else if ( m_constraintType == CONSTRAINT_FIXED ) + { + hk_Fixed_Constraint *pConstraint = (hk_Fixed_Constraint *)GetRealConstraint(); + if ( pConstraintToReference ) + { + ConvertHavanaLocalMatrixToHL( pConstraint->get_transform(0), *pConstraintToReference, NULL ); + } + if ( pConstraintToAttached ) + { + ConvertHavanaLocalMatrixToHL( pConstraint->get_transform(1), *pConstraintToAttached, NULL ); + } + return true; + } + return false; +} + +// header.pGroup is optionally NULL, all other fields must be valid! +static bool IsValidConstraint( const vphysics_save_cphysicsconstraint_t &header ) +{ + if ( header.constraintType != CONSTRAINT_UNKNOWN && header.pObjAttached && header.pObjReference ) + return true; + + return false; +} + + +bool CPhysicsConstraint::GetConstraintParams( constraint_breakableparams_t *pParams ) const +{ + if ( !pParams ) + return false; + vphysics_save_cphysicsconstraint_t header; + vphysics_save_constraint_t constraintTemplate; + memset( &header, 0, sizeof(header) ); + memset( &constraintTemplate, 0, sizeof(constraintTemplate) ); + WriteToTemplate( header, constraintTemplate ); + + if ( IsValidConstraint( header ) ) + { + switch ( header.constraintType ) + { + case CONSTRAINT_UNKNOWN: + break; + case CONSTRAINT_HINGE: + *pParams = constraintTemplate.hinge.constraint; + return true; + case CONSTRAINT_FIXED: + *pParams = constraintTemplate.fixed.constraint; + return true; + case CONSTRAINT_SLIDING: + *pParams = constraintTemplate.sliding.constraint; + return true; + case CONSTRAINT_PULLEY: + *pParams = constraintTemplate.pulley.constraint; + return true; + case CONSTRAINT_LENGTH: + *pParams = constraintTemplate.length.constraint; + return true; + case CONSTRAINT_BALLSOCKET: + *pParams = constraintTemplate.ballsocket.constraint; + return true; + case CONSTRAINT_RAGDOLL: + *pParams = constraintTemplate.ragdoll.constraint; + return true; + } + } + return false; +} + +CPhysicsConstraintGroup *CPhysicsConstraint::GetConstraintGroup() const +{ + if ( !m_HkConstraint ) + return NULL; + + hk_Local_Constraint_System *plcs = m_HkConstraint->get_constraint_system(); + Assert(plcs); + return (CPhysicsConstraintGroup *)plcs->get_client_data(); +} + +void CPhysicsConstraint::ReadBreakableConstraint( constraint_breakableparams_t ¶ms ) const +{ + if ( m_isBreakable ) + { + hk_Breakable_Constraint_BP bp; + ((hk_Breakable_Constraint *)m_HkConstraint)->write_to_blueprint( &bp ); + + params.forceLimit = ConvertDistanceToHL( bp.m_linear_strength ); + params.torqueLimit = RAD2DEG( bp.m_angular_strength ); + params.strength = 1.0; + params.bodyMassScale[0] = bp.m_bodyMassScale[0]; + params.bodyMassScale[1] = bp.m_bodyMassScale[1]; + //Assert( m_HkLCS != NULL ); // this is allowed now although breaking inside an LCS won't work yet + } + else + { + params.Defaults(); + } + + if ( m_HkLCS ) + { + params.isActive = m_HkLCS->is_active(); + } +} + + +void CPhysicsConstraint::WriteFixed( constraint_fixedparams_t &fixed ) const +{ + hk_Fixed_Constraint *pConstraint = (hk_Fixed_Constraint *)GetRealConstraint(); + ReadBreakableConstraint( fixed.constraint ); + + hk_Fixed_BP fixed_bp; + pConstraint->write_to_blueprint( &fixed_bp ); + // save fixed bp into params + ConvertHavanaLocalMatrixToHL( fixed_bp.m_transform_os_ks, fixed.attachedRefXform, m_pObjReference->GetObject() ); +} + +void CPhysicsConstraint::WriteRagdoll( constraint_ragdollparams_t &ragdoll ) const +{ + hk_Ragdoll_Constraint *pConstraint = (hk_Ragdoll_Constraint *)GetRealConstraint(); + ReadBreakableConstraint( ragdoll.constraint ); + hk_Ragdoll_Constraint_BP ragdoll_bp; + // BUGBUG: Write this and figure out how to recreate + // or change init func to setup this bp directly + pConstraint->write_to_blueprint( &ragdoll_bp ); + + ConvertHavanaLocalMatrixToHL( ragdoll_bp.m_transform_os_ks[0], ragdoll.constraintToReference, m_pObjReference->GetObject() ); + ConvertHavanaLocalMatrixToHL( ragdoll_bp.m_transform_os_ks[1], ragdoll.constraintToAttached, m_pObjAttached->GetObject() ); + int revAxisMapHK[3]; + revAxisMapHK[ragdoll_bp.m_axisMap[0]] = 0; + revAxisMapHK[ragdoll_bp.m_axisMap[1]] = 1; + revAxisMapHK[ragdoll_bp.m_axisMap[2]] = 2; + for ( int i = 0; i < 3; i ++ ) + { + constraint_axislimit_t &ragdollAxis = ragdoll.axes[i]; + int ivpAxis = ConvertCoordinateAxisToIVP( i ); + int hkAxis = revAxisMapHK[ ivpAxis ]; + hk_Constraint_Limit_BP &bpAxis = ragdoll_bp.m_limits[ hkAxis ]; + + ragdollAxis.angularVelocity = RAD2DEG(bpAxis.m_desired_velocity); + ragdollAxis.torque = bpAxis.m_joint_friction * m_pObjReference->GetInvMass(); + // X&Y + if ( i != 2 ) + { + ragdollAxis.minRotation = RAD2DEG(bpAxis.m_limit_min); + ragdollAxis.maxRotation = RAD2DEG(bpAxis.m_limit_max); + } + // Z + else + { + ragdollAxis.minRotation = -RAD2DEG(bpAxis.m_limit_max); + ragdollAxis.maxRotation = -RAD2DEG(bpAxis.m_limit_min); + } + } + ragdoll.childIndex = -1; + ragdoll.parentIndex = -1; + ragdoll.isActive = true; + ragdoll.onlyAngularLimits = ragdoll_bp.m_constrainTranslation ? false : true; + ragdoll.useClockwiseRotations = false; +} + +void CPhysicsConstraint::WriteHinge( constraint_hingeparams_t &hinge ) const +{ + hk_Hinge_Constraint *pConstraint = (hk_Hinge_Constraint *)GetRealConstraint(); + ReadBreakableConstraint( hinge.constraint ); + + hk_Hinge_BP hinge_bp; + pConstraint->write_to_blueprint( &hinge_bp ); + // save hinge bp into params + hinge.worldPosition = TransformHavanaLocalToHLWorld( hinge_bp.m_axis_os[0].get_origin(), m_pObjReference->GetObject(), true ); + hinge.worldAxisDirection = TransformHavanaLocalToHLWorld( hinge_bp.m_axis_os[0].get_direction(), m_pObjReference->GetObject(), false ); + hinge.hingeAxis.SetAxisFriction( 0,0,0 ); + + if ( hinge_bp.m_limit.m_limit_is_enabled ) + { + hinge.hingeAxis.minRotation = RAD2DEG(hinge_bp.m_limit.m_limit_min); + hinge.hingeAxis.maxRotation = RAD2DEG(hinge_bp.m_limit.m_limit_max); + } + if ( hinge_bp.m_limit.m_friction_is_enabled ) + { + hinge.hingeAxis.angularVelocity = RAD2DEG(hinge_bp.m_limit.m_desired_velocity); + hinge.hingeAxis.torque = RAD2DEG(hinge_bp.m_limit.m_joint_friction); + } +} + +void CPhysicsConstraint::WriteSliding( constraint_slidingparams_t &sliding ) const +{ + sliding.Defaults(); + hk_Prismatic_Constraint *pConstraint = (hk_Prismatic_Constraint *)GetRealConstraint(); + ReadBreakableConstraint( sliding.constraint ); + + hk_Prismatic_BP prismatic_bp; + pConstraint->write_to_blueprint( &prismatic_bp ); + // save bp into params + + hk_Transform t; + t.set_translation( prismatic_bp.m_transform_Ros_Aos.m_translation ); + t.set_rotation( prismatic_bp.m_transform_Ros_Aos.m_rotation ); + ConvertHavanaLocalMatrixToHL( t, sliding.attachedRefXform, m_pObjReference->GetObject() ); + if ( prismatic_bp.m_limit.m_friction_is_enabled ) + { + sliding.friction = ConvertDistanceToHL( prismatic_bp.m_limit.m_joint_friction ); + sliding.velocity = ConvertDistanceToHL( prismatic_bp.m_limit.m_desired_velocity ); + } + if ( prismatic_bp.m_limit.m_limit_is_enabled ) + { + sliding.limitMin = ConvertDistanceToHL( prismatic_bp.m_limit.m_limit_min ); + sliding.limitMax = ConvertDistanceToHL( prismatic_bp.m_limit.m_limit_max ); + } + ConvertDirectionToHL( prismatic_bp.m_axis_Ros, sliding.slideAxisRef ); +} + + +void CPhysicsConstraint::WritePulley( constraint_pulleyparams_t &pulley ) const +{ + pulley.Defaults(); + hk_Pulley_Constraint *pConstraint = (hk_Pulley_Constraint *)GetRealConstraint(); + ReadBreakableConstraint( pulley.constraint ); + + hk_Pulley_BP pulley_bp; + pConstraint->write_to_blueprint( &pulley_bp ); + + // save bp into params + for ( int i = 0; i < 2; i++ ) + { + ConvertPositionToHL( pulley_bp.m_worldspace_point[i], pulley.pulleyPosition[i] ); + ConvertPositionToHL( pulley_bp.m_translation_os_ks[i], pulley.objectPosition[i] ); + } + pulley.gearRatio = pulley_bp.m_gearing; + + pulley.totalLength = ConvertDistanceToHL(pulley_bp.m_length); + pulley.isRigid = pulley_bp.m_is_rigid; +} + + +void CPhysicsConstraint::WriteLength( constraint_lengthparams_t &length ) const +{ + length.Defaults(); + hk_Stiff_Spring_Constraint *pConstraint = (hk_Stiff_Spring_Constraint *)GetRealConstraint(); + ReadBreakableConstraint( length.constraint ); + + hk_Stiff_Spring_BP stiff_bp; + pConstraint->write_to_blueprint( &stiff_bp ); + + // save bp into params + for ( int i = 0; i < 2; i++ ) + { + ConvertPositionToHL( stiff_bp.m_translation_os_ks[i], length.objectPosition[i] ); + } + + length.totalLength = ConvertDistanceToHL(stiff_bp.m_length); + length.minLength = ConvertDistanceToHL(stiff_bp.m_min_length); +} + + +void CPhysicsConstraint::WriteBallsocket( constraint_ballsocketparams_t &ballsocket ) const +{ + ballsocket.Defaults(); + hk_Ball_Socket_Constraint *pConstraint = (hk_Ball_Socket_Constraint *)GetRealConstraint(); + ReadBreakableConstraint( ballsocket.constraint ); + + hk_Ball_Socket_BP ballsocket_bp; + pConstraint->write_to_blueprint( &ballsocket_bp ); + + // save bp into params + for ( int i = 0; i < 2; i++ ) + { + ConvertPositionToHL( ballsocket_bp.m_translation_os_ks[i], ballsocket.constraintPosition[i] ); + } +} + + +void CPhysicsConstraint::DetachListener() +{ + if ( !(m_pObjReference->CallbackFlags() & CALLBACK_NEVER_DELETED) ) + { + m_pObjReference->GetObject()->remove_listener_object( this ); + } + + if ( !(m_pObjAttached->CallbackFlags() & CALLBACK_NEVER_DELETED) ) + { + m_pObjAttached->GetObject()->remove_listener_object( this ); + } + + m_pObjReference = NULL; + m_pObjAttached = NULL; +} + +void CPhysicsConstraint::event_object_deleted( IVP_Event_Object *pEvent ) +{ + if ( m_HkLCS && pEvent->real_object->get_core()->physical_unmoveable ) + { + // HACKHACK: This makes the behavior consistent + m_HkLCS->core_is_going_to_be_deleted_event( pEvent->real_object->get_core() ); + } + DetachListener(); + // the underlying constraint is no longer valid, delete it. + delete m_HkConstraint; + m_HkConstraint = NULL; + delete m_HkLCS; + m_HkLCS = NULL; + if ( pEvent->environment ) + { + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)pEvent->environment->client_data; + pEnvironment->NotifyConstraintDisabled( this ); + } +} + + +CPhysicsConstraint::~CPhysicsConstraint( void ) +{ + // arg. There should be a better way to do this + if ( m_HkConstraint || m_HkLCS ) + { + DetachListener(); + delete m_HkLCS; + delete m_HkConstraint; + } +} + +void CPhysicsConstraint::Activate( void ) +{ + if ( m_HkLCS ) + { + m_HkLCS->activate(); + } +} + + +void CPhysicsConstraint::Deactivate( void ) +{ + if ( m_HkLCS ) + { + m_HkLCS->deactivate(); + } +} + + +void CPhysicsConstraint::SetupRagdollAxis( int axis, const constraint_axislimit_t &axisData, hk_Limited_Ball_Socket_BP *ballsocketBP ) +{ + // X & Y + if ( axis != 2 ) + { + ballsocketBP->m_angular_limits[ ConvertCoordinateAxisToIVP(axis) ].set( DEG2RAD(axisData.minRotation), DEG2RAD(axisData.maxRotation) ); + } + // Z + else + { + ballsocketBP->m_angular_limits[ ConvertCoordinateAxisToIVP(axis) ].set( DEG2RAD(-axisData.maxRotation), DEG2RAD(-axisData.minRotation) ); + } +} + + +// UNDONE: Keep this around to clip "includeStatic" code +#if 0 +void CPhysicsConstraint::SetBreakLimit( float breakLimitForce, float breakLimitTorque, bool includeStatic ) +{ + float factor = ConvertDistanceToIVP( 1.0f ); + + // convert to ivp + IVP_Environment *pEnvironment = m_pConstraint->get_associated_controlled_cores()->element_at(0)->environment; + float gravity = pEnvironment->get_gravity()->real_length(); + breakLimitTorque = breakLimitTorque * gravity * factor; // proportional to distance + breakLimitForce = breakLimitForce * gravity; + + if ( breakLimitForce != 0 ) + { + if ( includeStatic ) + { + breakLimitForce += m_pObjAttached->GetMass() * gravity * pEnvironment->get_delta_PSI_time(); + } + + m_pConstraint->change_max_translation_impulse( IVP_CFE_BREAK, breakLimitForce ); + } + else + { + m_pConstraint->change_max_translation_impulse( IVP_CFE_NONE, 0 ); + } + + if ( breakLimitTorque != 0 ) + { + if ( includeStatic ) + { + const IVP_U_Point *massCenter = m_pObjAttached->GetObject()->get_core()->get_position_PSI(); + + IVP_U_Point tmp; + tmp.set( massCenter ); + tmp.subtract( &m_constraintOrigin ); + float dist = tmp.real_length(); + breakLimitTorque += (m_pObjAttached->GetMass() * gravity * dist * pEnvironment->get_delta_PSI_time()); + } + m_pConstraint->change_max_rotation_impulse( IVP_CFE_BREAK, breakLimitTorque ); + } + else + { + m_pConstraint->change_max_rotation_impulse( IVP_CFE_NONE, 0 ); + } +} +#endif + + +IPhysicsObject *CPhysicsConstraint::GetReferenceObject( void ) const +{ + return m_pObjReference; +} + + +IPhysicsObject *CPhysicsConstraint::GetAttachedObject( void ) const +{ + return m_pObjAttached; +} + +void SeedRandomGenerators() +{ + ivp_srand(1); + hk_Math::srand01('h'+'a'+'v'+'o'+'k'); + qh_RANDOMseed_(1); +} + +extern int ivp_srand_read(void); +void ReadRandom( int buffer[4] ) +{ + buffer[0] = (int)hk_Math::hk_random_seed; + buffer[1] = ivp_srand_read(); +} + +void WriteRandom( int buffer[4] ) +{ + hk_Math::srand01((unsigned int)buffer[0]); + ivp_srand(buffer[1]); +} + + +IPhysicsConstraint *GetClientDataForHkConstraint( class hk_Breakable_Constraint *pHkConstraint ) +{ + return static_cast<CPhysicsConstraint *>( pHkConstraint->get_client_data() ); +} + +// Create a container for a group of constraints +IPhysicsConstraintGroup *CreatePhysicsConstraintGroup( IVP_Environment *pEnvironment, const constraint_groupparams_t &group ) +{ + MEM_ALLOC_CREDIT(); + return new CPhysicsConstraintGroup( pEnvironment, group ); +} + +IPhysicsConstraint *CreateRagdollConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_ragdollparams_t &ragdoll ) +{ + MEM_ALLOC_CREDIT(); + CPhysicsConstraint *pConstraint = new CPhysicsConstraint( pReferenceObject, pAttachedObject ); + pConstraint->InitRagdoll( pEnvironment, (CPhysicsConstraintGroup *)pGroup, ragdoll ); + return pConstraint; +} +IPhysicsConstraint *CreateHingeConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_limitedhingeparams_t &hinge ) +{ + MEM_ALLOC_CREDIT(); + CPhysicsConstraint *pConstraint = new CPhysicsConstraint( pReferenceObject, pAttachedObject ); + pConstraint->InitHinge( pEnvironment, (CPhysicsConstraintGroup *)pGroup, hinge ); + return pConstraint; +} + +IPhysicsConstraint *CreateFixedConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_fixedparams_t &fixed ) +{ + MEM_ALLOC_CREDIT(); + CPhysicsConstraint *pConstraint = new CPhysicsConstraint( pReferenceObject, pAttachedObject ); + pConstraint->InitFixed( pEnvironment, (CPhysicsConstraintGroup *)pGroup, fixed ); + return pConstraint; +} + +IPhysicsConstraint *CreateSlidingConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_slidingparams_t &sliding ) +{ + MEM_ALLOC_CREDIT(); + CPhysicsConstraint *pConstraint = new CPhysicsConstraint( pReferenceObject, pAttachedObject ); + pConstraint->InitSliding( pEnvironment, (CPhysicsConstraintGroup *)pGroup, sliding ); + return pConstraint; +} + +IPhysicsConstraint *CreateBallsocketConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_ballsocketparams_t &ballsocket ) +{ + MEM_ALLOC_CREDIT(); + CPhysicsConstraint *pConstraint = new CPhysicsConstraint( pReferenceObject, pAttachedObject ); + pConstraint->InitBallsocket( pEnvironment, (CPhysicsConstraintGroup *)pGroup, ballsocket ); + return pConstraint; +} + +IPhysicsConstraint *CreatePulleyConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_pulleyparams_t &pulley ) +{ + MEM_ALLOC_CREDIT(); + CPhysicsConstraint *pConstraint = new CPhysicsConstraint( pReferenceObject, pAttachedObject ); + pConstraint->InitPulley( pEnvironment, (CPhysicsConstraintGroup *)pGroup, pulley ); + return pConstraint; +} + +IPhysicsConstraint *CreateLengthConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_lengthparams_t &length ) +{ + MEM_ALLOC_CREDIT(); + CPhysicsConstraint *pConstraint = new CPhysicsConstraint( pReferenceObject, pAttachedObject ); + pConstraint->InitLength( pEnvironment, (CPhysicsConstraintGroup *)pGroup, length ); + return pConstraint; +} + +bool IsExternalConstraint( IVP_Controller *pLCS, void *pGameData ) +{ + IVP_U_Vector<IVP_Core> *pCores = pLCS->get_associated_controlled_cores(); + if ( pCores ) + { + for ( int i = 0; i < pCores->n_elems; i++ ) + { + if ( pCores->element_at(i) ) + { + IVP_Real_Object *pivp = pCores->element_at(i)->objects.element_at(0); + if ( pivp) + { + IPhysicsObject *pObject = static_cast<IPhysicsObject *>(pivp->client_data); + if ( pObject && pObject->GetGameData() != pGameData ) + return true; + } + } + } + } + return false; +} + +bool SavePhysicsConstraint( const physsaveparams_t ¶ms, CPhysicsConstraint *pConstraint ) +{ + vphysics_save_cphysicsconstraint_t header; + vphysics_save_constraint_t constraintTemplate; + memset( &header, 0, sizeof(header) ); + memset( &constraintTemplate, 0, sizeof(constraintTemplate) ); + + pConstraint->WriteToTemplate( header, constraintTemplate ); + + params.pSave->WriteAll( &header ); + if ( IsValidConstraint( header ) ) + { + switch ( header.constraintType ) + { + case CONSTRAINT_UNKNOWN: + Assert(0); + break; + case CONSTRAINT_HINGE: + params.pSave->WriteAll( &constraintTemplate.hinge ); + break; + case CONSTRAINT_FIXED: + params.pSave->WriteAll( &constraintTemplate.fixed ); + break; + case CONSTRAINT_SLIDING: + params.pSave->WriteAll( &constraintTemplate.sliding ); + break; + case CONSTRAINT_PULLEY: + params.pSave->WriteAll( &constraintTemplate.pulley ); + break; + case CONSTRAINT_LENGTH: + params.pSave->WriteAll( &constraintTemplate.length ); + break; + case CONSTRAINT_BALLSOCKET: + params.pSave->WriteAll( &constraintTemplate.ballsocket ); + break; + case CONSTRAINT_RAGDOLL: + params.pSave->WriteAll( &constraintTemplate.ragdoll ); + break; + } + return true; + } + // inert constraint, just save header + return true; +} + +bool RestorePhysicsConstraint( const physrestoreparams_t ¶ms, CPhysicsConstraint **ppConstraint ) +{ + vphysics_save_cphysicsconstraint_t header; + memset( &header, 0, sizeof(header) ); + + params.pRestore->ReadAll( &header ); + if ( IsValidConstraint( header ) ) + { + switch ( header.constraintType ) + { + case CONSTRAINT_UNKNOWN: + Assert(0); + break; + case CONSTRAINT_HINGE: + { + vphysics_save_constrainthinge_t hinge; + memset( &hinge, 0, sizeof(hinge) ); + params.pRestore->ReadAll( &hinge ); + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + *ppConstraint = (CPhysicsConstraint *)pEnvironment->CreateHingeConstraint( header.pObjReference, header.pObjAttached, header.pGroup, hinge ); + } + break; + case CONSTRAINT_FIXED: + { + vphysics_save_constraintfixed_t fixed; + memset( &fixed, 0, sizeof(fixed) ); + params.pRestore->ReadAll( &fixed ); + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + *ppConstraint = (CPhysicsConstraint *)pEnvironment->CreateFixedConstraint( header.pObjReference, header.pObjAttached, header.pGroup, fixed ); + } + break; + case CONSTRAINT_SLIDING: + { + vphysics_save_constraintsliding_t sliding; + memset( &sliding, 0, sizeof(sliding) ); + params.pRestore->ReadAll( &sliding ); + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + *ppConstraint = (CPhysicsConstraint *)pEnvironment->CreateSlidingConstraint( header.pObjReference, header.pObjAttached, header.pGroup, sliding ); + } + break; + case CONSTRAINT_PULLEY: + { + vphysics_save_constraintpulley_t pulley; + memset( &pulley, 0, sizeof(pulley) ); + params.pRestore->ReadAll( &pulley ); + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + *ppConstraint = (CPhysicsConstraint *)pEnvironment->CreatePulleyConstraint( header.pObjReference, header.pObjAttached, header.pGroup, pulley ); + } + break; + case CONSTRAINT_LENGTH: + { + vphysics_save_constraintlength_t length; + memset( &length, 0, sizeof(length) ); + params.pRestore->ReadAll( &length ); + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + *ppConstraint = (CPhysicsConstraint *)pEnvironment->CreateLengthConstraint( header.pObjReference, header.pObjAttached, header.pGroup, length ); + } + break; + case CONSTRAINT_BALLSOCKET: + { + vphysics_save_constraintballsocket_t ballsocket; + memset( &ballsocket, 0, sizeof(ballsocket) ); + params.pRestore->ReadAll( &ballsocket ); + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + *ppConstraint = (CPhysicsConstraint *)pEnvironment->CreateBallsocketConstraint( header.pObjReference, header.pObjAttached, header.pGroup, ballsocket ); + } + break; + case CONSTRAINT_RAGDOLL: + { + vphysics_save_constraintragdoll_t ragdoll; + memset( &ragdoll, 0, sizeof(ragdoll) ); + params.pRestore->ReadAll( &ragdoll ); + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + *ppConstraint = (CPhysicsConstraint *)pEnvironment->CreateRagdollConstraint( header.pObjReference, header.pObjAttached, header.pGroup, ragdoll ); + } + break; + } + + if ( *ppConstraint ) + { + (*ppConstraint)->SetGameData( params.pGameData ); + } + return true; + } + + // inert constraint, create an empty shell + *ppConstraint = new CPhysicsConstraint( NULL, NULL ); + return true; +} + + +bool SavePhysicsConstraintGroup( const physsaveparams_t ¶ms, CPhysicsConstraintGroup *pConstraintGroup ) +{ + vphysics_save_cphysicsconstraintgroup_t groupTemplate; + memset( &groupTemplate, 0, sizeof(groupTemplate) ); + + pConstraintGroup->WriteToTemplate( groupTemplate ); + params.pSave->WriteAll( &groupTemplate ); + return true; +} + +bool RestorePhysicsConstraintGroup( const physrestoreparams_t ¶ms, CPhysicsConstraintGroup **ppConstraintGroup ) +{ + vphysics_save_cphysicsconstraintgroup_t groupTemplate; + memset( &groupTemplate, 0, sizeof(groupTemplate) ); + params.pRestore->ReadAll( &groupTemplate ); + if ( groupTemplate.errorTolerance == 0.0f && groupTemplate.minErrorTicks == 0 ) + { + constraint_groupparams_t tmp; + tmp.Defaults(); + groupTemplate.minErrorTicks = tmp.minErrorTicks; + groupTemplate.errorTolerance = tmp.errorTolerance; + } + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + *ppConstraintGroup = (CPhysicsConstraintGroup *)pEnvironment->CreateConstraintGroup( groupTemplate ); + if ( *ppConstraintGroup && groupTemplate.isActive ) + { + g_ConstraintGroupActivateList.AddToTail( *ppConstraintGroup ); + } + return true; +} + + +void PostRestorePhysicsConstraintGroup() +{ + MEM_ALLOC_CREDIT(); + for ( int i = 0; i < g_ConstraintGroupActivateList.Count(); i++ ) + { + g_ConstraintGroupActivateList[i]->Activate(); + } + g_ConstraintGroupActivateList.Purge(); +} diff --git a/vphysics/physics_constraint.h b/vphysics/physics_constraint.h new file mode 100644 index 0000000..293dfbc --- /dev/null +++ b/vphysics/physics_constraint.h @@ -0,0 +1,32 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_CONSTRAINT_H +#define PHYSICS_CONSTRAINT_H +#pragma once + +class IVP_Environment; + +class CPhysicsObject; +class IPhysicsConstraint; +class IPhysicsConstraintGroup; + +extern IPhysicsConstraint *CreateRagdollConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_ragdollparams_t &ragdoll ); +extern IPhysicsConstraint *CreateHingeConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_limitedhingeparams_t &ragdoll ); +extern IPhysicsConstraint *CreateFixedConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_fixedparams_t &fixed ); +extern IPhysicsConstraint *CreateBallsocketConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_ballsocketparams_t &ballsocket ); +extern IPhysicsConstraint *CreateSlidingConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_slidingparams_t &slide ); +extern IPhysicsConstraint *CreatePulleyConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_pulleyparams_t &pulley ); +extern IPhysicsConstraint *CreateLengthConstraint( IVP_Environment *pEnvironment, CPhysicsObject *pReferenceObject, CPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_lengthparams_t &length ); + +extern IPhysicsConstraintGroup *CreatePhysicsConstraintGroup( IVP_Environment *pEnvironment, const constraint_groupparams_t &group ); + +extern IPhysicsConstraint *GetClientDataForHkConstraint( class hk_Breakable_Constraint *pHkConstraint ); + +extern bool IsExternalConstraint( IVP_Controller *pLCS, void *pGameData ); + +#endif // PHYSICS_CONSTRAINT_H diff --git a/vphysics/physics_controller_raycast_vehicle.cpp b/vphysics/physics_controller_raycast_vehicle.cpp new file mode 100644 index 0000000..a742fe8 --- /dev/null +++ b/vphysics/physics_controller_raycast_vehicle.cpp @@ -0,0 +1,171 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//=============================================================================// + +#include "cbase.h" +#include "physics_controller_raycast_vehicle.h" +#include "ivp_material.hxx" +#include "ivp_ray_solver.hxx" +#include "ivp_cache_object.hxx" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +//----------------------------------------------------------------------------- +// Purpose: Constructor +//----------------------------------------------------------------------------- +CPhysics_Car_System_Raycast_Wheels::CPhysics_Car_System_Raycast_Wheels( IVP_Environment *pEnv, + const IVP_Template_Car_System *pCarSystem ) + : IVP_Controller_Raycast_Car( pEnv, pCarSystem ) +{ + InitCarSystemWheels( pCarSystem ); +} + +//----------------------------------------------------------------------------- +// Purpose: Deconstructor +//----------------------------------------------------------------------------- +CPhysics_Car_System_Raycast_Wheels::~CPhysics_Car_System_Raycast_Wheels() +{ +} + +//----------------------------------------------------------------------------- +// Purpose: Setup the car system wheels. +//----------------------------------------------------------------------------- +void CPhysics_Car_System_Raycast_Wheels::InitCarSystemWheels( const IVP_Template_Car_System *pCarSystem ) +{ + for ( int iWheel = 0; iWheel < pCarSystem->n_wheels; ++iWheel ) + { + m_pWheels[iWheel] = pCarSystem->car_wheel[iWheel]; + m_pWheels[iWheel]->enable_collision_detection( IVP_FALSE ); + } +} + +//----------------------------------------------------------------------------- +// Purpose: Get the raycast wheel. +//----------------------------------------------------------------------------- +IPhysicsObject *CPhysics_Car_System_Raycast_Wheels::GetWheel( int index ) +{ + Assert( index >= 0 ); + Assert( index < n_wheels ); + + return ( IPhysicsObject* )m_pWheels[index]->client_data; +} + +//----------------------------------------------------------------------------- +// Purpose: Setup the car system wheels. +//----------------------------------------------------------------------------- +void CPhysics_Car_System_Raycast_Wheels::do_raycasts( IVP_Event_Sim *es, + int n_wheels, + class IVP_Ray_Solver_Template *t_in, + class IVP_Ray_Hit *hits_out, + IVP_FLOAT *friction_of_object_out ) +{ + t_in[0].ray_flags = IVP_RAY_SOLVER_ALL; + + int j = 0; + IVP_Ray_Solver_Min ray_solver0(&t_in[j]); + j++; if ( j >= n_wheels) j--; + IVP_Ray_Solver_Min ray_solver1(&t_in[j]); + j++; if ( j >= n_wheels) j--; + IVP_Ray_Solver_Min ray_solver2(&t_in[j]); + j++; if ( j >= n_wheels) j--; + IVP_Ray_Solver_Min ray_solver3(&t_in[j]); + + IVP_Ray_Solver_Min *solvers[4] = { &ray_solver0, &ray_solver1, &ray_solver2, &ray_solver3 }; + IVP_Ray_Solver_Group rs_group( n_wheels, (IVP_Ray_Solver **)solvers ); + +#if 0 + // Debug! + IVP_CarSystemDebugData_t carSystemDebugData; + GetCarSystemDebugData( carSystemDebugData ); + carSystemDebugData.wheelRaycasts[0][0] = ray_solver0.ray_start_point; + carSystemDebugData.wheelRaycasts[0][1] = ray_solver0.ray_end_point; + carSystemDebugData.wheelRaycasts[1][0] = ray_solver1.ray_start_point; + carSystemDebugData.wheelRaycasts[1][1] = ray_solver1.ray_end_point; + carSystemDebugData.wheelRaycasts[2][0] = ray_solver2.ray_start_point; + carSystemDebugData.wheelRaycasts[2][1] = ray_solver2.ray_end_point; + carSystemDebugData.wheelRaycasts[3][0] = ray_solver3.ray_start_point; + carSystemDebugData.wheelRaycasts[3][1] = ray_solver3.ray_end_point; +#endif + + // check which objects are hit + rs_group.check_ray_group_against_all_objects_in_sim(es->environment); + + for ( int i = 0; i < n_wheels; i++ ) + { + IVP_Ray_Hit *hit = solvers[i]->get_ray_hit(); + if (hit) + { + hits_out[i] = *hit; + friction_of_object_out[i] = hit->hit_real_object->l_default_material->get_friction_factor(); + +#if 0 + // Debug! + carSystemDebugData.wheelRaycastImpacts[i] = ( hit->hit_distance / solvers[i]->ray_length ); +#endif + } + else + { + memset( &hits_out[i], 0, sizeof(IVP_Ray_Hit) ); + friction_of_object_out[i] = 0; + +#if 0 + // Debug! + carSystemDebugData.wheelRaycastImpacts[i] = 0.0f; +#endif + } + } + +#if 0 + // Debug! + SetCarSystemDebugData( carSystemDebugData ); +#endif +} + +void CPhysics_Car_System_Raycast_Wheels::update_wheel_positions( void ) +{ + // Get the car body object. + IVP_Cache_Object *pCacheObject = car_body->get_cache_object(); + + // Get the core (vehicle) matrix. + IVP_U_Matrix m_core_f_object; + car_body->calc_m_core_f_object( &m_core_f_object ); + + for ( int iWheel = 0; iWheel < n_wheels; ++iWheel ) + { + // Get the current raycast wheel. + IVP_Raycast_Car_Wheel *pRaycastWheel = get_wheel( IVP_POS_WHEEL( iWheel ) ); + + // Get the position of the wheel in vehicle core space. + IVP_U_Float_Point hp_cs; + hp_cs.add_multiple( &pRaycastWheel->hp_cs, &pRaycastWheel->spring_direction_cs, pRaycastWheel->raycast_dist - pRaycastWheel->wheel_radius ); + + // Get the position on vehicle object space (inverse transform). + IVP_U_Float_Point hp_os; + m_core_f_object.vimult4( &hp_cs, &hp_os ); + + // Transform the wheel position from object space into world space. + IVP_U_Point hp_ws; + pCacheObject->transform_position_to_world_coords( &hp_os, &hp_ws ); + + // Apply rotational component. + IVP_U_Point wheel_cs( &pRaycastWheel->axis_direction_cs ); + IVP_U_Point wheel2_cs( 0 ,0 ,0 ); + wheel2_cs.k[index_y] = -1.0; + wheel2_cs.rotate( IVP_COORDINATE_INDEX( index_x ), pRaycastWheel->angle_wheel ); + + IVP_U_Matrix3 m_core_f_wheel; + m_core_f_wheel.init_normized3_col( &wheel_cs, IVP_COORDINATE_INDEX( index_x ), &wheel2_cs ); + + IVP_U_Matrix3 m_world_f_wheel; + pCacheObject->m_world_f_object.mmult3( &m_core_f_wheel, &m_world_f_wheel ); // bid hack, assumes cs = os (for rotation); + + IVP_U_Quat rot_ws; + rot_ws.set_quaternion( &m_world_f_wheel ); + m_pWheels[iWheel]->beam_object_to_new_position( &rot_ws, &hp_ws ); + } + + pCacheObject->remove_reference(); +} diff --git a/vphysics/physics_controller_raycast_vehicle.h b/vphysics/physics_controller_raycast_vehicle.h new file mode 100644 index 0000000..944c84d --- /dev/null +++ b/vphysics/physics_controller_raycast_vehicle.h @@ -0,0 +1,46 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//=============================================================================// + +#ifndef PHYSICS_CONTROLLER_RAYCAST_VEHICLE_H +#define PHYSICS_CONTROLLER_RAYCAST_VEHICLE_H +#ifdef _WIN32 +#pragma once +#endif + +#include "ivp_controller.hxx" +#include "ivp_car_system.hxx" +#include "ivp_controller_raycast_car.hxx" + +class IPhysicsObject; + +//============================================================================= +// +// Raycast Car System +// +class CPhysics_Car_System_Raycast_Wheels : public IVP_Controller_Raycast_Car +{ + +public: + + CPhysics_Car_System_Raycast_Wheels( IVP_Environment *env, const IVP_Template_Car_System *t ); + virtual ~CPhysics_Car_System_Raycast_Wheels(); + + virtual void do_raycasts( IVP_Event_Sim *, int n_wheels, IVP_Ray_Solver_Template *t_in, + IVP_Ray_Hit *hits_out, IVP_FLOAT *friction_of_object_out ); + + void update_wheel_positions( void ); + + IPhysicsObject *GetWheel( int index ); + + virtual const char *get_controller_name() { return "sys:vehicle"; } +protected: + + void InitCarSystemWheels( const IVP_Template_Car_System *pCarSystem ); + + IVP_Real_Object *m_pWheels[IVP_RAYCAST_CAR_MAX_WHEELS]; +}; + +#endif // PHYSICS_CONTROLLER_RAYCAST_VEHICLE_H diff --git a/vphysics/physics_environment.cpp b/vphysics/physics_environment.cpp new file mode 100644 index 0000000..c261f68 --- /dev/null +++ b/vphysics/physics_environment.cpp @@ -0,0 +1,2228 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// +#include "cbase.h" +#include "tier0/threadtools.h" +#include "physics_constraint.h" +#include "physics_spring.h" +#include "physics_fluid.h" +#include "physics_shadow.h" +#include "physics_motioncontroller.h" +#include "physics_vehicle.h" +#include "physics_virtualmesh.h" +#include "utlmultilist.h" +#include "vphysics/constraints.h" +#include "vphysics/vehicles.h" +#include "vphysics/object_hash.h" +#include "vphysics/performance.h" +#include "vphysics/stats.h" +#include "vphysics/player_controller.h" +#include "vphysics_saverestore.h" +#include "vphysics_internal.h" + +#include "ivu_linear_macros.hxx" +#include "ivp_collision_filter.hxx" +#include "ivp_listener_collision.hxx" +#include "ivp_listener_object.hxx" +#include "ivp_mindist.hxx" +#include "ivp_friction.hxx" +#include "ivp_anomaly_manager.hxx" +#include "ivp_time.hxx" +#include "ivp_listener_psi.hxx" +#include "ivp_phantom.hxx" +#include "ivp_range_manager.hxx" +#include "ivp_clustering_visualizer.hxx" +#include "ivp_mindist_intern.hxx" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +IPhysicsObjectPairHash *CreateObjectPairHash(); + +IVP_Synapse_Friction *GetOppositeSynapse( IVP_Synapse_Friction *pfriction ) +{ + IVP_Contact_Point *contact = pfriction->get_contact_point(); + IVP_Synapse_Friction *ptest = contact->get_synapse(0); + if ( ptest == pfriction ) + { + ptest = contact->get_synapse(1); + } + + return ptest; +} + +IVP_Real_Object *GetOppositeSynapseObject( IVP_Synapse_Friction *pfriction ) +{ + IVP_Synapse_Friction *opposite = GetOppositeSynapse( pfriction ); + return opposite->get_object(); +} + +// simple delete queue +class IDeleteQueueItem +{ +public: + // Add a virtual destructor to silence the clang warning. + // Note that this destructor doesn't actually do anything -- you + // still have to use the Delete() then delete pattern. + virtual ~IDeleteQueueItem() {} + virtual void Delete() = 0; +}; + +template <typename T> +class CDeleteProxy : public IDeleteQueueItem +{ +public: + CDeleteProxy(T *pItem) : m_pItem(pItem) {} + virtual void Delete() { delete m_pItem; } +private: + T *m_pItem; +}; + +class CDeleteQueue +{ +public: + void Add( IDeleteQueueItem *pItem ) + { + m_list.AddToTail( pItem ); + } + + template <typename T> + void QueueForDelete( T *pItem ) + { + Add( new CDeleteProxy<T>(pItem) ); + } + void DeleteAll() + { + for ( int i = m_list.Count()-1; i >= 0; --i) + { + m_list[i]->Delete(); + delete m_list[i]; + } + m_list.RemoveAll(); + } + +private: + CUtlVector< IDeleteQueueItem * > m_list; +}; + +class CPhysicsCollisionData : public IPhysicsCollisionData +{ +public: + CPhysicsCollisionData( IVP_Contact_Situation *contact ) : m_pContact(contact) {} + + virtual void GetSurfaceNormal( Vector &out ) { ConvertDirectionToHL( m_pContact->surf_normal, out ); } + virtual void GetContactPoint( Vector &out ) { ConvertPositionToHL( m_pContact->contact_point_ws, out ); } + virtual void GetContactSpeed( Vector &out ) { ConvertPositionToHL( m_pContact->speed, out ); } + + const IVP_Contact_Situation *m_pContact; +}; + +class CPhysicsFrictionData : public IPhysicsCollisionData +{ +public: + CPhysicsFrictionData( IVP_Synapse_Friction *synapse, float sign ) : m_sign(sign) + { + m_pPoint = synapse->get_contact_point(); + m_pContact = NULL; + } + + CPhysicsFrictionData( IVP_Event_Friction *pEvent ) : m_sign(1.0f) + { + m_pPoint = pEvent->friction_handle; + m_pContact = pEvent->contact_situation; + } + + virtual void GetSurfaceNormal( Vector &out ) + { + if ( m_pContact ) + { + ConvertDirectionToHL( m_pContact->surf_normal, out ); + } + else + { + IVP_U_Float_Point normal; + IVP_Contact_Point_API::get_surface_normal_ws(const_cast<IVP_Contact_Point *>(m_pPoint), &normal); + ConvertDirectionToHL( normal, out ); + out *= m_sign; + } + } + virtual void GetContactPoint( Vector &out ) + { + if ( m_pContact ) + { + ConvertPositionToHL( m_pContact->contact_point_ws, out ); + } + else + { + ConvertPositionToHL( *m_pPoint->get_contact_point_ws(), out ); + } + } + virtual void GetContactSpeed( Vector &out ) + { + if ( m_pContact ) + { + ConvertPositionToHL( m_pContact->speed, out ); + } + else + { + out.Init(); + } + } + +private: + const IVP_Contact_Point *m_pPoint; + float m_sign; + const IVP_Contact_Situation *m_pContact; +}; + + +//----------------------------------------------------------------------------- +// Purpose: Routes object event callbacks to game code +//----------------------------------------------------------------------------- +class CSleepObjects : public IVP_Listener_Object +{ +public: + CSleepObjects( void ) : IVP_Listener_Object() + { + m_pCallback = NULL; + m_lastScrapeTime = 0.0f; + } + + void SetHandler( IPhysicsObjectEvent *pListener ) + { + m_pCallback = pListener; + } + + void Remove( int index ) + { + // fast remove preserves indices except for the last element (moved into the empty spot) + m_activeObjects.FastRemove(index); + // If this isn't the last element, shift its index over + if ( index < m_activeObjects.Count() ) + { + m_activeObjects[index]->SetActiveIndex( index ); + } + } + + void DeleteObject( CPhysicsObject *pObject ) + { + int index = pObject->GetActiveIndex(); + if ( index < m_activeObjects.Count() ) + { + Assert( m_activeObjects[index] == pObject ); + Remove( index ); + pObject->SetActiveIndex( 0xFFFF ); + } + else + { + Assert(index==0xFFFF); + } + + } + + void event_object_deleted( IVP_Event_Object *pEvent ) + { + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pEvent->real_object->client_data); + if ( !pObject ) + return; + + DeleteObject(pObject); + } + + void event_object_created( IVP_Event_Object *pEvent ) + { + } + + void event_object_revived( IVP_Event_Object *pEvent ) + { + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pEvent->real_object->client_data); + if ( !pObject ) + return; + + int sleepState = pObject->GetSleepState(); + + pObject->NotifyWake(); + + // asleep, but already in active list + if ( sleepState == OBJ_STARTSLEEP ) + return; + + // don't track static objects (like the world). That way we only track objects that will move + if ( pObject->GetObject()->get_movement_state() != IVP_MT_STATIC ) + { + Assert(pObject->GetActiveIndex()==0xFFFF); + if ( pObject->GetActiveIndex()!=0xFFFF) + return; + + int index = m_activeObjects.AddToTail( pObject ); + pObject->SetActiveIndex( index ); + } + if ( m_pCallback ) + { + m_pCallback->ObjectWake( pObject ); + } + } + + void event_object_frozen( IVP_Event_Object *pEvent ) + { + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pEvent->real_object->client_data); + if ( !pObject ) + return; + + pObject->NotifySleep(); + if ( m_pCallback ) + { + m_pCallback->ObjectSleep( pObject ); + } + } + + //----------------------------------------------------------------------------- + // Purpose: This walks the objects in the environment and generates friction events + // for any scraping that is occurring. + //----------------------------------------------------------------------------- + void ProcessActiveObjects( IVP_Environment *pEnvironment, IPhysicsCollisionEvent *pEvent ) + { + // FIXME: Is this correct? Shouldn't it do next PSI - lastScrape? + float nextTime = pEnvironment->get_old_time_of_last_PSI().get_time(); + float delta = nextTime - m_lastScrapeTime; + + // only process if we have done a PSI + if ( delta < pEnvironment->get_delta_PSI_time() ) + return; + + float t = 0.0f; + if ( delta != 0.0f ) + { + t = 1.0f / delta; + } + + m_lastScrapeTime = nextTime; + + // UNDONE: This only calls friciton for one object in each pair. + // UNDONE: Split energy in half and call for both objects? + // UNDONE: Don't split/call if one object is static (like the world)? + for ( int i = 0; i < m_activeObjects.Count(); i++ ) + { + CPhysicsObject *pObject = m_activeObjects[i]; + IVP_Real_Object *ivpObject = pObject->GetObject(); + + // no friction callbacks for this object + if ( ! (pObject->CallbackFlags() & CALLBACK_GLOBAL_FRICTION) ) + continue; + + // UNDONE: IVP_Synapse_Friction is supposed to be opaque. Is there a better way + // to implement this? Using the friction listener is much more work for the CPU + // and considers sleeping objects. + IVP_Synapse_Friction *pfriction = ivpObject->get_first_friction_synapse(); + while ( pfriction ) + { + IVP_Contact_Point *contact = pfriction->get_contact_point(); + IVP_Synapse_Friction *pOpposite = GetOppositeSynapse( pfriction ); + IVP_Real_Object *pobj = pOpposite->get_object(); + CPhysicsObject *pScrape = (CPhysicsObject *)pobj->client_data; + + // friction callbacks for this object? + if ( pScrape->CallbackFlags() & CALLBACK_GLOBAL_FRICTION ) + { + float energy = IVP_Contact_Point_API::get_eliminated_energy( contact ); + if ( energy ) + { + // scrape with an estimate for the energy per unit mass + // This assumes that the game is interested in some measure of vibration + // for sound effects. This also assumes that more massive objects require + // more energy to vibrate. + energy = energy * t * ivpObject->get_core()->get_inv_mass(); + + if ( energy > 0.05f ) + { + int hitSurface = pScrape->GetMaterialIndexInternal(); + + int materialIndex = pOpposite->get_material_index(); + if ( materialIndex ) + { + // use the per-triangle material if it has one + hitSurface = physprops->RemapIVPMaterialIndex( materialIndex ); + } + + float sign = (pfriction == contact->get_synapse(0)) ? 1 : -1; + + CPhysicsFrictionData data(pfriction, sign); + + pEvent->Friction( pObject, ConvertEnergyToHL(energy), pObject->GetMaterialIndexInternal(), hitSurface, &data ); + } + IVP_Contact_Point_API::reset_eliminated_energy( contact ); + } + } + pfriction = pfriction->get_next(); + } + } + } + void DebugCheckContacts( IVP_Environment *pEnvironment ) + { + IVP_Mindist_Manager *pManager = pEnvironment->get_mindist_manager(); + + for( IVP_Mindist *mdist = pManager->exact_mindists; mdist != NULL; mdist = mdist->next ) + { + IVP_Real_Object *obj[2]; + mdist->get_objects( obj ); + IVP_BOOL check = pEnvironment->get_collision_filter()->check_objects_for_collision_detection( obj[0], obj[1] ); + Assert(check); + if ( !check ) + { + Msg("Changed collision rules for %s vs. %s without calling recheck!\n", obj[0]->get_name(), obj[1]->get_name() ); + } + } + } + int GetActiveObjectCount( void ) const + { + return m_activeObjects.Count(); + } + void GetActiveObjects( IPhysicsObject **pOutputObjectList ) const + { + for ( int i = 0; i < m_activeObjects.Count(); i++ ) + { + pOutputObjectList[i] = m_activeObjects[i]; + } + } + void UpdateSleepObjects( void ) + { + int i; + + CUtlVector<CPhysicsObject *> sleepObjects; + + for ( i = 0; i < m_activeObjects.Count(); i++ ) + { + CPhysicsObject *pObject = m_activeObjects[i]; + + if ( pObject->GetSleepState() != OBJ_AWAKE ) + { + sleepObjects.AddToTail( pObject ); + } + } + + for ( i = sleepObjects.Count()-1; i >= 0; --i ) + { + // put fully to sleep + sleepObjects[i]->NotifySleep(); + + // remove from the active list + DeleteObject( sleepObjects[i] ); + } + } + +private: + CUtlVector<CPhysicsObject *> m_activeObjects; + float m_lastScrapeTime; + IPhysicsObjectEvent *m_pCallback; +}; + +class CEmptyCollisionListener : public IPhysicsCollisionEvent +{ +public: + virtual void PreCollision( vcollisionevent_t *pEvent ) {} + virtual void PostCollision( vcollisionevent_t *pEvent ) {} + + // This is a scrape event. The object has scraped across another object consuming the indicated energy + virtual void Friction( IPhysicsObject *pObject, float energy, int surfaceProps, int surfacePropsHit, IPhysicsCollisionData *pData ) {} + + virtual void StartTouch( IPhysicsObject *pObject1, IPhysicsObject *pObject2, IPhysicsCollisionData *pTouchData ) {} + virtual void EndTouch( IPhysicsObject *pObject1, IPhysicsObject *pObject2, IPhysicsCollisionData *pTouchData ) {} + + virtual void FluidStartTouch( IPhysicsObject *pObject, IPhysicsFluidController *pFluid ) {} + virtual void FluidEndTouch( IPhysicsObject *pObject, IPhysicsFluidController *pFluid ) {} + + virtual void ObjectEnterTrigger( IPhysicsObject *pTrigger, IPhysicsObject *pObject ) {} + virtual void ObjectLeaveTrigger( IPhysicsObject *pTrigger, IPhysicsObject *pObject ) {} + + virtual void PostSimulationFrame() {} +}; + +CEmptyCollisionListener g_EmptyCollisionListener; + +#define ALL_COLLISION_FLAGS (IVP_LISTENER_COLLISION_CALLBACK_PRE_COLLISION|IVP_LISTENER_COLLISION_CALLBACK_POST_COLLISION|IVP_LISTENER_COLLISION_CALLBACK_FRICTION) +//----------------------------------------------------------------------------- +// Purpose: Routes collision event callbacks to game code +//----------------------------------------------------------------------------- +class CPhysicsListenerCollision : public IVP_Listener_Collision, public IVP_Listener_Phantom +{ +public: + CPhysicsListenerCollision(); + + void SetHandler( IPhysicsCollisionEvent *pCallback ) + { + m_pCallback = pCallback; + } + IPhysicsCollisionEvent *GetHandler() { return m_pCallback; } + + virtual void event_pre_collision( IVP_Event_Collision *pEvent ) + { + m_event.isCollision = false; + m_event.isShadowCollision = false; + IVP_Contact_Situation *contact = pEvent->contact_situation; + CPhysicsObject *pObject1 = static_cast<CPhysicsObject *>(contact->objects[0]->client_data); + CPhysicsObject *pObject2 = static_cast<CPhysicsObject *>(contact->objects[1]->client_data); + if ( !pObject1 || !pObject2 ) + return; + + unsigned int flags1 = pObject1->CallbackFlags(); + unsigned int flags2 = pObject2->CallbackFlags(); + + m_event.isCollision = (flags1 & flags2 & CALLBACK_GLOBAL_COLLISION) ? true : false; + + // only call shadow collisions if one is shadow and the other isn't (hence the xor) + // (if both are shadow, the collisions happen in AI - if neither, then no callback) + m_event.isShadowCollision = ((flags1^flags2) & CALLBACK_SHADOW_COLLISION) ? true : false; + + m_event.pObjects[0] = pObject1; + m_event.pObjects[1] = pObject2; + m_event.deltaCollisionTime = pEvent->d_time_since_last_collision; + // This timer must have been reset or something (constructor initializes time to -1000) + // Fake the time to 50ms (resets happen often in rolling collisions for some reason) + if ( m_event.deltaCollisionTime > 999 ) + { + m_event.deltaCollisionTime = 1.0; + } + + + CPhysicsCollisionData data(contact); + m_event.pInternalData = &data; + + // clear out any static object collisions unless flagged to keep them + if ( contact->objects[0]->get_movement_state() == IVP_MT_STATIC ) + { + // don't call global if disabled + if ( !(flags2 & CALLBACK_GLOBAL_COLLIDE_STATIC) ) + { + m_event.isCollision = false; + } + } + if ( contact->objects[1]->get_movement_state() == IVP_MT_STATIC ) + { + // don't call global if disabled + if ( !(flags1 & CALLBACK_GLOBAL_COLLIDE_STATIC) ) + { + m_event.isCollision = false; + } + } + + if ( !m_event.isCollision && !m_event.isShadowCollision ) + return; + + // look up surface props + for ( int i = 0; i < 2; i++ ) + { + m_event.surfaceProps[i] = physprops->GetIVPMaterialIndex( contact->materials[i] ); + if ( m_event.surfaceProps[i] < 0 ) + { + m_event.surfaceProps[i] = m_event.pObjects[i]->GetMaterialIndex(); + } + } + + m_pCallback->PreCollision( &m_event ); + } + + virtual void event_post_collision( IVP_Event_Collision *pEvent ) + { + // didn't call preCollision, so don't call postCollision + if ( !m_event.isCollision && !m_event.isShadowCollision ) + return; + + IVP_Contact_Situation *contact = pEvent->contact_situation; + + float collisionSpeed = contact->speed.dot_product(&contact->surf_normal); + m_event.collisionSpeed = ConvertDistanceToHL( fabs(collisionSpeed) ); + CPhysicsCollisionData data(contact); + m_event.pInternalData = &data; + + m_pCallback->PostCollision( &m_event ); + } + + virtual void event_collision_object_deleted( class IVP_Real_Object *) + { + // enable this in constructor + } + + virtual void event_friction_created( IVP_Event_Friction *pEvent ) + { + IVP_Contact_Situation *contact = pEvent->contact_situation; + CPhysicsObject *pObject1 = static_cast<CPhysicsObject *>(contact->objects[0]->client_data); + CPhysicsObject *pObject2 = static_cast<CPhysicsObject *>(contact->objects[1]->client_data); + + if ( !pObject1 || !pObject2 ) + return; + + unsigned int flags1 = pObject1->CallbackFlags(); + unsigned int flags2 = pObject2->CallbackFlags(); + unsigned int allflags = flags1|flags2; + + if ( !pObject1->IsStatic() || !pObject2->IsStatic() ) + { + if ( !pObject1->HasTouchedDynamic() && pObject2->IsMoveable() ) + { + pObject1->SetTouchedDynamic(); + } + if ( !pObject2->HasTouchedDynamic() && pObject1->IsMoveable() ) + { + pObject2->SetTouchedDynamic(); + } + } + + bool calltouch = ( allflags & CALLBACK_GLOBAL_TOUCH ) ? true : false; + if ( !calltouch ) + return; + + if ( pObject1->IsStatic() || pObject2->IsStatic() ) + { + if ( !( allflags & CALLBACK_GLOBAL_TOUCH_STATIC ) ) + return; + } + + CPhysicsFrictionData data(pEvent); + m_pCallback->StartTouch( pObject1, pObject2, &data ); + } + + + virtual void event_friction_deleted( IVP_Event_Friction *pEvent ) + { + IVP_Contact_Situation *contact = pEvent->contact_situation; + CPhysicsObject *pObject1 = static_cast<CPhysicsObject *>(contact->objects[0]->client_data); + CPhysicsObject *pObject2 = static_cast<CPhysicsObject *>(contact->objects[1]->client_data); + if ( !pObject1 || !pObject2 ) + return; + + unsigned int flags1 = pObject1->CallbackFlags(); + unsigned int flags2 = pObject2->CallbackFlags(); + + unsigned int allflags = flags1|flags2; + + bool calltouch = ( allflags & CALLBACK_GLOBAL_TOUCH ) ? true : false; + if ( !calltouch ) + return; + + if ( pObject1->IsStatic() || pObject2->IsStatic() ) + { + if ( !( allflags & CALLBACK_GLOBAL_TOUCH_STATIC ) ) + return; + } + + CPhysicsFrictionData data(pEvent); + m_pCallback->EndTouch( pObject1, pObject2, &data ); + } + + virtual void event_friction_pair_created( class IVP_Friction_Core_Pair *pair ); + virtual void event_friction_pair_deleted( class IVP_Friction_Core_Pair *pair ); + virtual void mindist_entered_volume( class IVP_Controller_Phantom *controller,class IVP_Mindist_Base *mindist ) {} + virtual void mindist_left_volume(class IVP_Controller_Phantom *controller, class IVP_Mindist_Base *mindist) {} + + virtual void core_entered_volume( IVP_Controller_Phantom *controller, IVP_Core *pCore ) + { + CPhysicsFluidController *pFluid = static_cast<CPhysicsFluidController *>( controller->client_data ); + IVP_Real_Object *pivp = pCore->objects.element_at(0); + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pivp->client_data); + if ( !pObject ) + return; + + if ( pFluid ) + { + if ( pObject && (pObject->CallbackFlags() & CALLBACK_FLUID_TOUCH) ) + { + m_pCallback->FluidStartTouch( pObject, pFluid ); + } + } + else + { + // must be a trigger + IVP_Real_Object *pTriggerIVP = controller->get_object(); + CPhysicsObject *pTrigger = static_cast<CPhysicsObject *>(pTriggerIVP->client_data); + + if ( pTrigger ) + { + m_pCallback->ObjectEnterTrigger( pTrigger, pObject ); + } + } + } + + virtual void core_left_volume( IVP_Controller_Phantom *controller, IVP_Core *pCore ) + { + CPhysicsFluidController *pFluid = static_cast<CPhysicsFluidController *>( controller->client_data ); + IVP_Real_Object *pivp = pCore->objects.element_at(0); + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pivp->client_data); + if ( !pObject ) + return; + + if ( pFluid ) + { + if ( pObject && (pObject->CallbackFlags() & CALLBACK_FLUID_TOUCH) ) + { + m_pCallback->FluidEndTouch( pObject, pFluid ); + } + } + else + { + // must be a trigger + IVP_Real_Object *pTriggerIVP = controller->get_object(); + CPhysicsObject *pTrigger = static_cast<CPhysicsObject *>(pTriggerIVP->client_data); + + if ( pTrigger ) + { + m_pCallback->ObjectLeaveTrigger( pTrigger, pObject ); + } + } + } + void phantom_is_going_to_be_deleted_event(class IVP_Controller_Phantom *controller) {} + + void EventPSI( CPhysicsEnvironment *pEnvironment ) + { + m_pCallback->PostSimulationFrame(); + UpdatePairListPSI( pEnvironment ); + } +private: + + struct corepair_t + { + corepair_t() {} + corepair_t( IVP_Friction_Core_Pair *pair ) + { + int index = ( pair->objs[0] < pair->objs[1] ) ? 0 : 1; + core0 = pair->objs[index]; + core1 = pair->objs[!index]; + lastImpactTime= pair->last_impact_time_pair; + } + + IVP_Core *core0; + IVP_Core *core1; + IVP_Time lastImpactTime; + }; + + static bool CorePairLessFunc( const corepair_t &lhs, const corepair_t &rhs ) + { + if ( lhs.core0 != rhs.core0 ) + return ( lhs.core0 < rhs.core0 ); + else + return ( lhs.core1 < rhs.core1 ); + } + void UpdatePairListPSI( CPhysicsEnvironment *pEnvironment ) + { + unsigned short index = m_pairList.FirstInorder(); + IVP_Time currentTime = pEnvironment->GetIVPEnvironment()->get_current_time(); + + while ( m_pairList.IsValidIndex(index) ) + { + unsigned short next = m_pairList.NextInorder( index ); + corepair_t &test = m_pairList.Element(index); + + // only keep 1 seconds worth of data + if ( (currentTime - test.lastImpactTime) > 1.0 ) + { + m_pairList.RemoveAt( index ); + } + index = next; + } + } + + CUtlRBTree<corepair_t> m_pairList; + float m_pairListOldestTime; + + + IPhysicsCollisionEvent *m_pCallback; + vcollisionevent_t m_event; + +}; + + +CPhysicsListenerCollision::CPhysicsListenerCollision() : IVP_Listener_Collision( ALL_COLLISION_FLAGS ), m_pCallback(&g_EmptyCollisionListener) +{ + m_pairList.SetLessFunc( CorePairLessFunc ); +} + + +void CPhysicsListenerCollision::event_friction_pair_created( IVP_Friction_Core_Pair *pair ) +{ + corepair_t test(pair); + unsigned short index = m_pairList.Find( test ); + if ( m_pairList.IsValidIndex( index ) ) + { + corepair_t &save = m_pairList.Element(index); + // found this one already, update the time + if ( save.lastImpactTime.get_seconds() > pair->last_impact_time_pair.get_seconds() ) + { + pair->last_impact_time_pair = save.lastImpactTime; + } + else + { + save.lastImpactTime = pair->last_impact_time_pair; + } + } + else + { + if ( m_pairList.Count() < 16 ) + { + m_pairList.Insert( test ); + } + } +} + + +void CPhysicsListenerCollision::event_friction_pair_deleted( IVP_Friction_Core_Pair *pair ) +{ + corepair_t test(pair); + unsigned short index = m_pairList.Find( test ); + if ( m_pairList.IsValidIndex( index ) ) + { + corepair_t &save = m_pairList.Element(index); + // found this one already, update the time + if ( save.lastImpactTime.get_seconds() < pair->last_impact_time_pair.get_seconds() ) + { + save.lastImpactTime = pair->last_impact_time_pair; + } + } + else + { + if ( m_pairList.Count() < 16 ) + { + m_pairList.Insert( test ); + } + } +} + + +#if IVP_ENABLE_VISUALIZER + +class CCollisionVisualizer : public IVP_Clustering_Visualizer_Shortrange_Callback, public IVP_Clustering_Visualizer_Longrange_Callback +{ + IVPhysicsDebugOverlay *m_pDebug; +public: + CCollisionVisualizer(IVPhysicsDebugOverlay *pDebug) { m_pDebug = pDebug;} + + void visualize_request() + { + Vector origin, extents; + ConvertPositionToHL( center, origin ); + float hlradius = ConvertDistanceToHL( radius); + extents.Init( hlradius, hlradius, hlradius ); + m_pDebug->AddBoxOverlay( origin, -extents, extents, vec3_angle, 0, 255, 0, 32, 0.5f); + } + + virtual void devisualize_request() {} + virtual void enable() {} + virtual void disable() {} + + void visualize_request_for_node() + { + Vector origin, extents; + ConvertPositionToHL( position, origin ); + ConvertPositionToHL( box_extents, extents ); + + Vector boxOrigin, boxExtents; + CPhysicsObject *pObject0 = static_cast<CPhysicsObject *>(node_object->client_data); + pObject0->LocalToWorld( boxOrigin, origin ); + QAngle angles; + pObject0->GetPosition( NULL, &angles ); + + m_pDebug->AddBoxOverlay( boxOrigin, -extents, extents, angles, 255, 255, 0, 0, 0.5f); + } + + void visualize_request_for_intruder_radius() + { + Vector origin, extents; + ConvertPositionToHL( position, origin ); + float hlradius = ConvertDistanceToHL( sphere_radius ); + + extents.Init( hlradius, hlradius, hlradius ); + m_pDebug->AddBoxOverlay( origin, -extents, extents, vec3_angle, 0, 0, 255, 32, 0.25f); + } +}; +#endif + +class CCollisionSolver : public IVP_Collision_Filter, public IVP_Anomaly_Manager +{ +public: + CCollisionSolver( void ) : IVP_Anomaly_Manager(IVP_FALSE) { m_pSolver = NULL; } + void SetHandler( IPhysicsCollisionSolver *pSolver ) { m_pSolver = pSolver; } + + // IVP_Collision_Filter + IVP_BOOL check_objects_for_collision_detection(IVP_Real_Object *ivp0, IVP_Real_Object *ivp1) + { + if ( m_pSolver ) + { + CPhysicsObject *pObject0 = static_cast<CPhysicsObject *>(ivp0->client_data); + CPhysicsObject *pObject1 = static_cast<CPhysicsObject *>(ivp1->client_data); + if ( pObject0 && pObject1 ) + { + if ( (pObject0->CallbackFlags() & CALLBACK_ENABLING_COLLISION) && (pObject1->CallbackFlags() & CALLBACK_MARKED_FOR_DELETE) ) + return IVP_FALSE; + + if ( (pObject1->CallbackFlags() & CALLBACK_ENABLING_COLLISION) && (pObject0->CallbackFlags() & CALLBACK_MARKED_FOR_DELETE) ) + return IVP_FALSE; + + if ( !m_pSolver->ShouldCollide( pObject0, pObject1, pObject0->GetGameData(), pObject1->GetGameData() ) ) + return IVP_FALSE; + } + } + return IVP_TRUE; + } + void environment_will_be_deleted(IVP_Environment *) {} + + // IVP_Anomaly_Manager + virtual void inter_penetration( IVP_Mindist *mindist,IVP_Real_Object *ivp0, IVP_Real_Object *ivp1, IVP_DOUBLE speedChange) + { + if ( m_pSolver ) + { + // UNDONE: project current velocity onto rescue velocity instead + // This will cause escapes to be slow - which is probably a good + // thing. That's probably a better heuristic than only rescuing once + // per PSI! + CPhysicsObject *pObject0 = static_cast<CPhysicsObject *>(ivp0->client_data); + CPhysicsObject *pObject1 = static_cast<CPhysicsObject *>(ivp1->client_data); + if ( pObject0 && pObject1 ) + { + if ( (pObject0->CallbackFlags() & CALLBACK_MARKED_FOR_DELETE) || + (pObject1->CallbackFlags() & CALLBACK_MARKED_FOR_DELETE) ) + return; + + // moveable object pair? + if ( pObject0->IsMoveable() && pObject1->IsMoveable() ) + { + // only push each pair apart once per PSI + if ( CheckObjPair( ivp0, ivp1 ) ) + return; + } + IVP_Environment *env = ivp0->get_environment(); + float deltaTime = env->get_delta_PSI_time(); + + if ( !m_pSolver->ShouldSolvePenetration( pObject0, pObject1, pObject0->GetGameData(), pObject1->GetGameData(), deltaTime ) ) + return; + } + else + { + return; + } + } + + IVP_Anomaly_Manager::inter_penetration( mindist, ivp0, ivp1, speedChange ); + } + + // return true if object should be temp. freezed + virtual IVP_BOOL max_collisions_exceeded_check_freezing(IVP_Anomaly_Limits *, IVP_Core *pCore) + { + if ( m_pSolver ) + { + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pCore->objects.element_at(0)->client_data); + return m_pSolver->ShouldFreezeObject( pObject ) ? IVP_TRUE : IVP_FALSE; + } + return IVP_TRUE; + } + // return number of additional checks to do this psi + virtual int max_collision_checks_exceeded( int totalChecks ) + { + if ( m_pSolver ) + { + return m_pSolver->AdditionalCollisionChecksThisTick( totalChecks ); + } + return 0; + } + void max_velocity_exceeded(IVP_Anomaly_Limits *al, IVP_Core *pCore, IVP_U_Float_Point *velocity_in_out) + { + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pCore->objects.element_at(0)->client_data); + if ( pObject->GetShadowController() != NULL ) + return; + IVP_Anomaly_Manager::max_velocity_exceeded(al, pCore, velocity_in_out); + } + IVP_BOOL max_contacts_exceeded_check_freezing( IVP_Core **pCoreList, int coreCount ) + { + CUtlVector<IPhysicsObject *> list; + list.EnsureCapacity(coreCount); + for ( int i = 0; i < coreCount; i++ ) + { + IVP_Core *pCore = pCoreList[i]; + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pCore->objects.element_at(0)->client_data); + list.AddToTail(pObject); + } + + return m_pSolver->ShouldFreezeContacts( list.Base(), list.Count() ) ? IVP_TRUE : IVP_FALSE; + } + + +public: + void EventPSI( CPhysicsEnvironment * ) + { + m_rescue.RemoveAll(); + } + + +private: + struct realobjectpair_t + { + IVP_Real_Object *pObj0; + IVP_Real_Object *pObj1; + inline bool operator==( const realobjectpair_t &src ) const + { + return (pObj0 == src.pObj0) && (pObj1 == src.pObj1); + } + }; + // basically each moveable object pair gets 1 rescue per PSI + // UNDONE: Add a counter to do more? + bool CheckObjPair( IVP_Real_Object *pObj0, IVP_Real_Object *pObj1 ) + { + realobjectpair_t tmp; + tmp.pObj0 = pObj0 < pObj1 ? pObj0 : pObj1; + tmp.pObj1 = pObj0 > pObj1 ? pObj0 : pObj1; + + if ( m_rescue.Find( tmp ) != m_rescue.InvalidIndex() ) + return true; + m_rescue.AddToTail( tmp ); + return false; + } + +private: + IPhysicsCollisionSolver *m_pSolver; + // UNDONE: Linear search? should be small, but switch to rb tree if this ever gets large + CUtlVector<realobjectpair_t> m_rescue; +#if IVP_ENABLE_VISUALIZER +public: + CCollisionVisualizer *pVisualizer; +#endif +}; + + + +class CPhysicsListenerConstraint : public IVP_Listener_Constraint +{ +public: + CPhysicsListenerConstraint() + { + m_pCallback = NULL; + } + + void SetHandler( IPhysicsConstraintEvent *pHandler ) + { + m_pCallback = pHandler; + } + + void event_constraint_broken( IVP_Constraint *pConstraint ) + { + // IVP_Constraint is not allowed, something is broken + Assert(0); + } + + void event_constraint_broken( hk_Breakable_Constraint *pConstraint ) + { + if ( m_pCallback ) + { + IPhysicsConstraint *pObj = GetClientDataForHkConstraint( pConstraint ); + m_pCallback->ConstraintBroken( pObj ); + } + } + void event_constraint_broken( IPhysicsConstraint *pConstraint ) + { + if ( m_pCallback ) + { + m_pCallback->ConstraintBroken(pConstraint); + } + } +private: + IPhysicsConstraintEvent *m_pCallback; +}; + + +#define AIR_DENSITY 2 + +class CDragController : public IVP_Controller_Independent +{ +public: + + CDragController( void ) + { + m_airDensity = AIR_DENSITY; + } + virtual ~CDragController( void ) {} + + virtual void do_simulation_controller(IVP_Event_Sim *event,IVP_U_Vector<IVP_Core> *core_list) + { + int i; + for( i = core_list->len()-1; i >=0; i--) + { + IVP_Core *pCore = core_list->element_at(i); + + IVP_Real_Object *pivp = pCore->objects.element_at(0); + CPhysicsObject *pPhys = static_cast<CPhysicsObject *>(pivp->client_data); + + float dragForce = -0.5 * pPhys->GetDragInDirection( pCore->speed ) * m_airDensity * event->delta_time; + if ( dragForce < -1.0f ) + dragForce = -1.0f; + if ( dragForce < 0 ) + { + IVP_U_Float_Point dragVelocity; + dragVelocity.set_multiple( &pCore->speed, dragForce ); + pCore->speed.add( &dragVelocity ); + } + float angDragForce = -pPhys->GetAngularDragInDirection( pCore->rot_speed ) * m_airDensity * event->delta_time; + if ( angDragForce < -1.0f ) + angDragForce = -1.0f; + if ( angDragForce < 0 ) + { + IVP_U_Float_Point angDragVelocity; + angDragVelocity.set_multiple( &pCore->rot_speed, angDragForce ); + pCore->rot_speed.add( &angDragVelocity ); + } + } + } + virtual const char *get_controller_name() { return "vphysics:drag"; } + + virtual IVP_CONTROLLER_PRIORITY get_controller_priority() + { + return IVP_CP_MOTION; + } + float GetAirDensity() const { return m_airDensity; } + void SetAirDensity( float density ) { m_airDensity = density; } + +private: + float m_airDensity; +}; + +// +// Default implementation of the debug overlay interface so that we never return NULL from GetDebugOverlay. +// +class CVPhysicsDebugOverlay : public IVPhysicsDebugOverlay +{ +public: + virtual void AddEntityTextOverlay(int ent_index, int line_offset, float duration, int r, int g, int b, int a, const char *format, ...) {} + virtual void AddBoxOverlay(const Vector& origin, const Vector& mins, const Vector& max, QAngle const& orientation, int r, int g, int b, int a, float duration) {} + virtual void AddTriangleOverlay(const Vector& p1, const Vector& p2, const Vector& p3, int r, int g, int b, int a, bool noDepthTest, float duration) {} + virtual void AddLineOverlay(const Vector& origin, const Vector& dest, int r, int g, int b,bool noDepthTest, float duration) {} + virtual void AddTextOverlay(const Vector& origin, float duration, const char *format, ...) {} + virtual void AddTextOverlay(const Vector& origin, int line_offset, float duration, const char *format, ...) {} + virtual void AddScreenTextOverlay(float flXPos, float flYPos,float flDuration, int r, int g, int b, int a, const char *text) {} + virtual void AddSweptBoxOverlay(const Vector& start, const Vector& end, const Vector& mins, const Vector& max, const QAngle & angles, int r, int g, int b, int a, float flDuration) {} + virtual void AddTextOverlayRGB(const Vector& origin, int line_offset, float duration, float r, float g, float b, float alpha, const char *format, ...) {} +}; + +static CVPhysicsDebugOverlay s_DefaultDebugOverlay; + + +CPhysicsEnvironment::CPhysicsEnvironment( void ) +// assume that these lists will have at least one object +{ + // set this to true to force the + m_deleteQuick = false; + m_queueDeleteObject = false; + m_inSimulation = false; + m_fixedTimestep = true; // try to simulate using fixed timesteps + m_enableConstraintNotify = false; + + // build a default environment + IVP_Environment_Manager *env_manager; + env_manager = IVP_Environment_Manager::get_environment_manager(); + + IVP_Application_Environment appl_env; + m_pCollisionSolver = new CCollisionSolver; + appl_env.collision_filter = m_pCollisionSolver; + appl_env.material_manager = physprops->GetIVPManager(); + appl_env.anomaly_manager = m_pCollisionSolver; + // UNDONE: This would save another 45K of RAM on xbox, test perf + // if ( IsXbox() ) + // { + // appl_env.n_cache_object = 128; + // } + + + BEGIN_IVP_ALLOCATION(); + m_pPhysEnv = env_manager->create_environment( &appl_env, "JAY", 0xBEEF ); + END_IVP_ALLOCATION(); + + // UNDONE: Revisit brush/terrain/object shrinking and tune this number to something larger + // UNDONE: Expose this to callers, also via physcollision + m_pPhysEnv->set_global_collision_tolerance( ConvertDistanceToIVP( g_PhysicsUnits.globalCollisionTolerance - 1e-4f ) ); // just under 1/4 inch tolerance + m_pSleepEvents = new CSleepObjects; + + m_pDeleteQueue = new CDeleteQueue; + + BEGIN_IVP_ALLOCATION(); + m_pPhysEnv->add_listener_object_global( m_pSleepEvents ); + END_IVP_ALLOCATION(); + + m_pCollisionListener = new CPhysicsListenerCollision; + + BEGIN_IVP_ALLOCATION(); + m_pPhysEnv->add_listener_collision_global( m_pCollisionListener ); + END_IVP_ALLOCATION(); + + m_pConstraintListener = new CPhysicsListenerConstraint; + + BEGIN_IVP_ALLOCATION(); + m_pPhysEnv->add_listener_constraint_global( m_pConstraintListener ); + END_IVP_ALLOCATION(); + + m_pDragController = new CDragController; + + physics_performanceparams_t perf; + perf.Defaults(); + SetPerformanceSettings( &perf ); + m_pPhysEnv->client_data = (void *)this; + m_lastObjectThisTick = 0; +} + +CPhysicsEnvironment::~CPhysicsEnvironment( void ) +{ + // no callbacks during shutdown + SetCollisionSolver( NULL ); + m_pPhysEnv->remove_listener_object_global( m_pSleepEvents ); + + // don't bother waking up other objects as we clear them out + SetQuickDelete( true ); + + // delete/remove the listeners + m_pPhysEnv->remove_listener_collision_global( m_pCollisionListener ); + delete m_pCollisionListener; + m_pPhysEnv->remove_listener_constraint_global( m_pConstraintListener ); + delete m_pConstraintListener; + + // Clean out the list of physics objects + for ( int i = m_objects.Count()-1; i >= 0; --i ) + { + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(m_objects[i]); + PhantomRemove( pObject ); + delete pObject; + } + + m_objects.RemoveAll(); + ClearDeadObjects(); + + // Clean out the list of fluids + m_fluids.PurgeAndDeleteElements(); + + delete m_pSleepEvents; + delete m_pDragController; + delete m_pPhysEnv; + delete m_pDeleteQueue; + + // must be deleted after the environment (calls back in destructor) + delete m_pCollisionSolver; +} + +IPhysicsCollisionEvent *CPhysicsEnvironment::GetCollisionEventHandler() +{ + return m_pCollisionListener->GetHandler(); +} + +void CPhysicsEnvironment::NotifyConstraintDisabled( IPhysicsConstraint *pConstraint ) +{ + if ( m_enableConstraintNotify ) + { + m_pConstraintListener->event_constraint_broken( pConstraint ); + } +} + +void CPhysicsEnvironment::DebugCheckContacts(void) +{ + if ( m_pSleepEvents ) + { + m_pSleepEvents->DebugCheckContacts( m_pPhysEnv ); + } +} + +void CPhysicsEnvironment::SetDebugOverlay( CreateInterfaceFn debugOverlayFactory ) +{ + m_pDebugOverlay = NULL; + if (debugOverlayFactory) + { + m_pDebugOverlay = ( IVPhysicsDebugOverlay * )debugOverlayFactory( VPHYSICS_DEBUG_OVERLAY_INTERFACE_VERSION, NULL ); + } + + if (!m_pDebugOverlay) + { + m_pDebugOverlay = &s_DefaultDebugOverlay; + } + +#if IVP_ENABLE_VISUALIZER + m_pCollisionSolver->pVisualizer = new CCollisionVisualizer( m_pDebugOverlay ); + INSTALL_SHORTRANGE_CALLBACK(m_pCollisionSolver->pVisualizer); + INSTALL_LONGRANGE_CALLBACK(m_pCollisionSolver->pVisualizer); + +#endif +} + + +IVPhysicsDebugOverlay *CPhysicsEnvironment::GetDebugOverlay( void ) +{ + return m_pDebugOverlay; +} + + +void CPhysicsEnvironment::SetGravity( const Vector& gravityVector ) +{ + IVP_U_Point gravity; + + ConvertPositionToIVP( gravityVector, gravity ); + m_pPhysEnv->set_gravity( &gravity ); + // BUGBUG: global collision tolerance has a constant that depends on gravity. + m_pPhysEnv->set_global_collision_tolerance( m_pPhysEnv->get_global_collision_tolerance(), gravity.real_length() ); + DevMsg(1,"Set Gravity %.1f (%.3f tolerance)\n", gravityVector.Length(), IVP2HL(m_pPhysEnv->get_global_collision_tolerance()) ); +} + + +void CPhysicsEnvironment::GetGravity( Vector *pGravityVector ) const +{ + const IVP_U_Point *gravity = m_pPhysEnv->get_gravity(); + + ConvertPositionToHL( *gravity, *pGravityVector ); +} + + +IPhysicsObject *CPhysicsEnvironment::CreatePolyObject( const CPhysCollide *pCollisionModel, int materialIndex, const Vector& position, const QAngle& angles, objectparams_t *pParams ) +{ + IPhysicsObject *pObject = ::CreatePhysicsObject( this, pCollisionModel, materialIndex, position, angles, pParams, false ); + if ( pObject ) + { + m_objects.AddToTail( pObject ); + } + return pObject; +} + +IPhysicsObject *CPhysicsEnvironment::CreatePolyObjectStatic( const CPhysCollide *pCollisionModel, int materialIndex, const Vector& position, const QAngle& angles, objectparams_t *pParams ) +{ + IPhysicsObject *pObject = ::CreatePhysicsObject( this, pCollisionModel, materialIndex, position, angles, pParams, true ); + if ( pObject ) + { + m_objects.AddToTail( pObject ); + } + return pObject; +} + +unsigned int CPhysicsEnvironment::GetObjectSerializeSize( IPhysicsObject *pObject ) const +{ + return sizeof(vphysics_save_cphysicsobject_t); +} + +void CPhysicsEnvironment::SerializeObjectToBuffer( IPhysicsObject *pObject, unsigned char *pBuffer, unsigned int bufferSize ) +{ + CPhysicsObject *pPhysics = static_cast<CPhysicsObject *>(pObject); + if ( bufferSize >= sizeof(vphysics_save_cphysicsobject_t)) + { + vphysics_save_cphysicsobject_t *pTemplate = reinterpret_cast<vphysics_save_cphysicsobject_t *>(pBuffer); + pPhysics->WriteToTemplate( *pTemplate ); + } +} + +IPhysicsObject *CPhysicsEnvironment::UnserializeObjectFromBuffer( void *pGameData, unsigned char *pBuffer, unsigned int bufferSize, bool enableCollisions ) +{ + IPhysicsObject *pObject = ::CreateObjectFromBuffer( this, pGameData, pBuffer, bufferSize, enableCollisions ); + if ( pObject ) + { + m_objects.AddToTail( pObject ); + } + return pObject; +} + +const IPhysicsObject **CPhysicsEnvironment::GetObjectList( int *pOutputObjectCount ) const +{ + int iCount = m_objects.Count(); + if( pOutputObjectCount ) + *pOutputObjectCount = iCount; + + if( iCount ) + return (const IPhysicsObject **)m_objects.Base(); + else + return NULL; +} + + + +extern void ControlPhysicsShadowControllerAttachment_Silent( IPhysicsShadowController *pController, IVP_Real_Object *pivp, bool bAttach ); +extern void ControlPhysicsPlayerControllerAttachment_Silent( IPhysicsPlayerController *pController, IVP_Real_Object *pivp, bool bAttach ); + +bool CPhysicsEnvironment::TransferObject( IPhysicsObject *pObject, IPhysicsEnvironment *pDestinationEnvironment ) +{ + int iIndex = m_objects.Find( pObject ); + if( iIndex == -1 || (pObject->GetCallbackFlags() & CALLBACK_MARKED_FOR_DELETE ) ) + return false; + + CPhysicsObject *pPhysics = static_cast<CPhysicsObject *>(pObject); + //pPhysics->Wake(); + //pPhysics->NotifyWake(); + + void *pGameData = pObject->GetGameData(); + + //Find any controllers attached to this object + IPhysicsShadowController *pController = pObject->GetShadowController(); + IPhysicsPlayerController *pPlayerController = NULL; + + if( (pObject->GetCallbackFlags() & CALLBACK_IS_PLAYER_CONTROLLER) != 0 ) + { + pPlayerController = FindPlayerController( pObject ); + } + + + + + //temporarily (and silently) detach any physics controllers we found because destroying the object would destroy them + if( pController ) + { + //detach the controller from the object + ((CPhysicsObject *)pObject)->m_pShadow = NULL; + + IVP_Real_Object *pivp = ((CPhysicsObject *)pObject)->GetObject(); + ControlPhysicsShadowControllerAttachment_Silent( pController, pivp, false ); + } + else if( pPlayerController ) + { + RemovePlayerController( pPlayerController ); + pObject->SetCallbackFlags( pObject->GetCallbackFlags() & ~CALLBACK_IS_PLAYER_CONTROLLER ); + + IVP_Real_Object *pivp = ((CPhysicsObject *)pObject)->GetObject(); + ControlPhysicsPlayerControllerAttachment_Silent( pPlayerController, pivp, false ); + } + + + //templatize the object + vphysics_save_cphysicsobject_t objectTemplate; + memset( &objectTemplate, 0, sizeof( vphysics_save_cphysicsobject_t ) ); + pPhysics->WriteToTemplate( objectTemplate ); + + //these should be detached already + Assert( objectTemplate.pShadow == NULL ); + Assert( objectTemplate.hasShadowController == false ); + + //destroy the existing version of the object + m_objects.FastRemove( iIndex ); + pPhysics->ForceSilentDelete(); + m_pSleepEvents->DeleteObject( pPhysics ); + pPhysics->CPhysicsObject::~CPhysicsObject(); + + //now recreate in place in the destination environment + CPhysicsEnvironment *pDest = static_cast<CPhysicsEnvironment *>(pDestinationEnvironment); + CreateObjectFromBuffer_UseExistingMemory( pDest, pGameData, (unsigned char *)&objectTemplate, sizeof(objectTemplate), pPhysics ); + pDest->m_objects.AddToTail( pObject ); + + //even if this is going to sleep in a second, put it active right away to fix some object hitching problems + pPhysics->Wake(); + pPhysics->NotifyWake(); + /*int iActiveIndex = pDest->m_pSleepEvents->m_activeObjects.AddToTail( pPhysics ); + pPhysics->SetActiveIndex( iActiveIndex );*/ + + pDest->m_pPhysEnv->force_psi_on_next_simulation(); //avoids an object pause + + if( pController ) + { + //re-attach the controller to the new object + ((CPhysicsObject *)pObject)->m_pShadow = pController; + + IVP_Real_Object *pivp = ((CPhysicsObject *)pObject)->GetObject(); + ControlPhysicsShadowControllerAttachment_Silent( pController, pivp, true ); + } + else if( pPlayerController ) + { + IVP_Real_Object *pivp = ((CPhysicsObject *)pObject)->GetObject(); + pObject->SetCallbackFlags( pObject->GetCallbackFlags() | CALLBACK_IS_PLAYER_CONTROLLER ); + ControlPhysicsPlayerControllerAttachment_Silent( pPlayerController, pivp, true ); + + pDest->AddPlayerController( pPlayerController ); + } + + return true; +} + + +IPhysicsSpring *CPhysicsEnvironment::CreateSpring( IPhysicsObject *pObjectStart, IPhysicsObject *pObjectEnd, springparams_t *pParams ) +{ + return ::CreateSpring( m_pPhysEnv, static_cast<CPhysicsObject *>(pObjectStart), static_cast<CPhysicsObject *>(pObjectEnd), pParams ); +} + +IPhysicsFluidController *CPhysicsEnvironment::CreateFluidController( IPhysicsObject *pFluidObject, fluidparams_t *pParams ) +{ + CPhysicsFluidController *pFluid = ::CreateFluidController( m_pPhysEnv, static_cast<CPhysicsObject *>(pFluidObject), pParams ); + m_fluids.AddToTail( pFluid ); + return pFluid; +} + +IPhysicsConstraint *CPhysicsEnvironment::CreateRagdollConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_ragdollparams_t &ragdoll ) +{ + return ::CreateRagdollConstraint( m_pPhysEnv, (CPhysicsObject *)pReferenceObject, (CPhysicsObject *)pAttachedObject, pGroup, ragdoll ); +} + +IPhysicsConstraint *CPhysicsEnvironment::CreateHingeConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_hingeparams_t &hinge ) +{ + constraint_limitedhingeparams_t limitedhinge(hinge); + return ::CreateHingeConstraint( m_pPhysEnv, (CPhysicsObject *)pReferenceObject, (CPhysicsObject *)pAttachedObject, pGroup, limitedhinge ); +} + +IPhysicsConstraint *CPhysicsEnvironment::CreateLimitedHingeConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_limitedhingeparams_t &hinge ) +{ + return ::CreateHingeConstraint( m_pPhysEnv, (CPhysicsObject *)pReferenceObject, (CPhysicsObject *)pAttachedObject, pGroup, hinge ); +} + +IPhysicsConstraint *CPhysicsEnvironment::CreateFixedConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_fixedparams_t &fixed ) +{ + return ::CreateFixedConstraint( m_pPhysEnv, (CPhysicsObject *)pReferenceObject, (CPhysicsObject *)pAttachedObject, pGroup, fixed ); +} + +IPhysicsConstraint *CPhysicsEnvironment::CreateSlidingConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_slidingparams_t &sliding ) +{ + return ::CreateSlidingConstraint( m_pPhysEnv, (CPhysicsObject *)pReferenceObject, (CPhysicsObject *)pAttachedObject, pGroup, sliding ); +} + +IPhysicsConstraint *CPhysicsEnvironment::CreateBallsocketConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_ballsocketparams_t &ballsocket ) +{ + return ::CreateBallsocketConstraint( m_pPhysEnv, (CPhysicsObject *)pReferenceObject, (CPhysicsObject *)pAttachedObject, pGroup, ballsocket ); +} + +IPhysicsConstraint *CPhysicsEnvironment::CreatePulleyConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_pulleyparams_t &pulley ) +{ + return ::CreatePulleyConstraint( m_pPhysEnv, (CPhysicsObject *)pReferenceObject, (CPhysicsObject *)pAttachedObject, pGroup, pulley ); +} + +IPhysicsConstraint *CPhysicsEnvironment::CreateLengthConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_lengthparams_t &length ) +{ + return ::CreateLengthConstraint( m_pPhysEnv, (CPhysicsObject *)pReferenceObject, (CPhysicsObject *)pAttachedObject, pGroup, length ); +} + +IPhysicsConstraintGroup *CPhysicsEnvironment::CreateConstraintGroup( const constraint_groupparams_t &group ) +{ + return CreatePhysicsConstraintGroup( m_pPhysEnv, group ); +} + +void CPhysicsEnvironment::Simulate( float deltaTime ) +{ + LOCAL_THREAD_LOCK(); + + if ( !m_pPhysEnv ) + return; + + ClearDeadObjects(); +#if DEBUG_CHECK_CONTATCTS_AUTO + m_pSleepEvents->DebugCheckContacts( m_pPhysEnv ); +#endif + + // save this to catch objects deleted without being simulated + m_lastObjectThisTick = m_objects.Count()-1; + + // stop updating objects that went to sleep during the previous frame. + m_pSleepEvents->UpdateSleepObjects(); + + // Trap interrupts and clock changes + // don't simulate less than .1 ms + if ( deltaTime <= 1.0 && deltaTime > 0.0001 ) + { + if ( deltaTime > 0.1 ) + { + deltaTime = 0.1f; + } + + m_pCollisionSolver->EventPSI( this ); + m_pCollisionListener->EventPSI( this ); + + m_inSimulation = true; + BEGIN_IVP_ALLOCATION(); + if ( !m_fixedTimestep || deltaTime != m_pPhysEnv->get_delta_PSI_time() ) + { + m_fixedTimestep = false; + m_pPhysEnv->simulate_dtime( deltaTime ); + } + else + { + m_pPhysEnv->simulate_time_step(); + } + END_IVP_ALLOCATION(); + m_inSimulation = false; + } + + // If the queue is disabled, it's only used during simulation. + // Flush it as soon as possible (which is now) + if ( !m_queueDeleteObject ) + { + ClearDeadObjects(); + } + + if ( m_pCollisionListener->GetHandler() ) + { + m_pSleepEvents->ProcessActiveObjects( m_pPhysEnv, m_pCollisionListener->GetHandler() ); + } + VISUALIZE_COLLISIONS(); + VirtualMeshPSI(); + GetNextFrameTime(); +} + +void CPhysicsEnvironment::ResetSimulationClock() +{ + // UNDONE: You'd think that all of this would make the system deterministic, but + // it doesn't. + extern void SeedRandomGenerators(); + + m_pPhysEnv->reset_time(); + m_pPhysEnv->get_time_manager()->env_set_current_time( m_pPhysEnv, IVP_Time(0) ); + m_pPhysEnv->reset_time(); + m_fixedTimestep = true; + SeedRandomGenerators(); +} + +float CPhysicsEnvironment::GetSimulationTimestep( void ) const +{ + return m_pPhysEnv->get_delta_PSI_time(); +} + +void CPhysicsEnvironment::SetSimulationTimestep( float timestep ) +{ + m_pPhysEnv->set_delta_PSI_time( timestep ); +} + +float CPhysicsEnvironment::GetSimulationTime( void ) const +{ + return (float)m_pPhysEnv->get_current_time().get_time(); +} + +float CPhysicsEnvironment::GetNextFrameTime( void ) const +{ + return (float)m_pPhysEnv->get_next_PSI_time().get_time(); +} + + +// true if currently running the simulator (i.e. in a callback during physenv->Simulate()) +bool CPhysicsEnvironment::IsInSimulation( void ) const +{ + return m_inSimulation; +} + +void CPhysicsEnvironment::DestroyObject( IPhysicsObject *pObject ) +{ + if ( !pObject ) + { + DevMsg("Deleted NULL vphysics object\n"); + return; + } + + // search from the end because we usually delete the most recent objects during run time + int index = -1; + for ( int i = m_objects.Count(); --i >= 0; ) + { + if ( m_objects[i] == pObject ) + { + index = i; + break; + } + } + + if ( index != -1 ) + { + m_objects.FastRemove( index ); + } + else + { + DevMsg(1,"error deleting physics object\n"); + CPhysicsObject *pPhysics = static_cast<CPhysicsObject *>(pObject); + if ( pPhysics->GetCallbackFlags() & CALLBACK_MARKED_FOR_DELETE ) + { + // deleted twice + Assert(0); + return; + } + // bad ptr? + Assert(0); + return; + } + + CPhysicsObject *pPhysics = static_cast<CPhysicsObject *>(pObject); + // add this flag so we can optimize some cases + pPhysics->AddCallbackFlags( CALLBACK_MARKED_FOR_DELETE ); + + // was created/destroyed without simulating. No need to wake the neighbors! + if ( index > m_lastObjectThisTick ) + { + pPhysics->ForceSilentDelete(); + } + + if ( m_inSimulation || m_queueDeleteObject ) + { + // don't delete while simulating + m_deadObjects.AddToTail( pObject ); + } + else + { + m_pSleepEvents->DeleteObject( pPhysics ); + delete pObject; + } +} + +void CPhysicsEnvironment::DestroySpring( IPhysicsSpring *pSpring ) +{ + delete pSpring; +} +void CPhysicsEnvironment::DestroyFluidController( IPhysicsFluidController *pFluid ) +{ + m_fluids.FindAndRemove( (CPhysicsFluidController *)pFluid ); + delete pFluid; +} + + +void CPhysicsEnvironment::DestroyConstraint( IPhysicsConstraint *pConstraint ) +{ + if ( !m_deleteQuick && pConstraint ) + { + IPhysicsObject *pObj0 = pConstraint->GetReferenceObject(); + if ( pObj0 ) + { + pObj0->Wake(); + } + + IPhysicsObject *pObj1 = pConstraint->GetAttachedObject(); + if ( pObj1 ) + { + pObj1->Wake(); + } + } + if ( m_inSimulation ) + { + pConstraint->Deactivate(); + m_pDeleteQueue->QueueForDelete( pConstraint ); + } + else + { + delete pConstraint; + } +} + +void CPhysicsEnvironment::DestroyConstraintGroup( IPhysicsConstraintGroup *pGroup ) +{ + delete pGroup; +} + +void CPhysicsEnvironment::TraceBox( trace_t *ptr, const Vector &mins, const Vector &maxs, const Vector &start, const Vector &end ) +{ + // UNDONE: Need this? +} + +void CPhysicsEnvironment::SetCollisionSolver( IPhysicsCollisionSolver *pSolver ) +{ + m_pCollisionSolver->SetHandler( pSolver ); +} + + +void CPhysicsEnvironment::ClearDeadObjects( void ) +{ + for ( int i = 0; i < m_deadObjects.Count(); i++ ) + { + CPhysicsObject *pObject = (CPhysicsObject *)m_deadObjects.Element(i); + + m_pSleepEvents->DeleteObject( pObject ); + delete pObject; + } + m_deadObjects.Purge(); + m_pDeleteQueue->DeleteAll(); +} + +void CPhysicsEnvironment::AddPlayerController( IPhysicsPlayerController *pController ) +{ + if ( m_playerControllers.Find(pController) != -1 ) + { + Assert(0); + return; + } + m_playerControllers.AddToTail( pController ); +} + +void CPhysicsEnvironment::RemovePlayerController( IPhysicsPlayerController *pController ) +{ + m_playerControllers.FindAndRemove( pController ); +} + +IPhysicsPlayerController *CPhysicsEnvironment::FindPlayerController( IPhysicsObject *pPhysicsObject ) +{ + for ( int i = m_playerControllers.Count()-1; i >= 0; --i ) + { + if ( m_playerControllers[i]->GetObject() == pPhysicsObject ) + return m_playerControllers[i]; + } + return NULL; +} + + +void CPhysicsEnvironment::SetCollisionEventHandler( IPhysicsCollisionEvent *pCollisionEvents ) +{ + m_pCollisionListener->SetHandler( pCollisionEvents ); +} + + +void CPhysicsEnvironment::SetObjectEventHandler( IPhysicsObjectEvent *pObjectEvents ) +{ + m_pSleepEvents->SetHandler( pObjectEvents ); +} + +void CPhysicsEnvironment::SetConstraintEventHandler( IPhysicsConstraintEvent *pConstraintEvents ) +{ + m_pConstraintListener->SetHandler( pConstraintEvents ); +} + + +IPhysicsShadowController *CPhysicsEnvironment::CreateShadowController( IPhysicsObject *pObject, bool allowTranslation, bool allowRotation ) +{ + return ::CreateShadowController( static_cast<CPhysicsObject*>(pObject), allowTranslation, allowRotation ); +} + +void CPhysicsEnvironment::DestroyShadowController( IPhysicsShadowController *pController ) +{ + delete pController; +} + +IPhysicsPlayerController *CPhysicsEnvironment::CreatePlayerController( IPhysicsObject *pObject ) +{ + IPhysicsPlayerController *pController = ::CreatePlayerController( static_cast<CPhysicsObject*>(pObject) ); + AddPlayerController( pController ); + return pController; +} + +void CPhysicsEnvironment::DestroyPlayerController( IPhysicsPlayerController *pController ) +{ + RemovePlayerController( pController ); + ::DestroyPlayerController( pController ); +} + +IPhysicsMotionController *CPhysicsEnvironment::CreateMotionController( IMotionEvent *pHandler ) +{ + return ::CreateMotionController( this, pHandler ); +} + +void CPhysicsEnvironment::DestroyMotionController( IPhysicsMotionController *pController ) +{ + delete pController; +} + +IPhysicsVehicleController *CPhysicsEnvironment::CreateVehicleController( IPhysicsObject *pVehicleBodyObject, const vehicleparams_t ¶ms, unsigned int nVehicleType, IPhysicsGameTrace *pGameTrace ) +{ + return ::CreateVehicleController( this, static_cast<CPhysicsObject*>(pVehicleBodyObject), params, nVehicleType, pGameTrace ); +} + +void CPhysicsEnvironment::DestroyVehicleController( IPhysicsVehicleController *pController ) +{ + delete pController; +} + +int CPhysicsEnvironment::GetActiveObjectCount( void ) const +{ + return m_pSleepEvents->GetActiveObjectCount(); +} + + +void CPhysicsEnvironment::GetActiveObjects( IPhysicsObject **pOutputObjectList ) const +{ + m_pSleepEvents->GetActiveObjects( pOutputObjectList ); +} + +void CPhysicsEnvironment::SetAirDensity( float density ) +{ + CDragController *pDrag = ((CDragController *)m_pDragController); + if ( pDrag ) + { + pDrag->SetAirDensity( density ); + } +} + +float CPhysicsEnvironment::GetAirDensity( void ) const +{ + const CDragController *pDrag = ((CDragController *)m_pDragController); + if ( pDrag ) + { + return pDrag->GetAirDensity(); + } + return 0; +} + +void CPhysicsEnvironment::CleanupDeleteList() +{ + ClearDeadObjects(); +} + +bool CPhysicsEnvironment::IsCollisionModelUsed( CPhysCollide *pCollide ) const +{ + int i; + + for ( i = m_deadObjects.Count()-1; i >= 0; --i ) + { + if ( m_deadObjects[i]->GetCollide() == pCollide ) + return true; + } + + for ( i = m_objects.Count()-1; i >= 0; --i ) + { + if ( m_objects[i]->GetCollide() == pCollide ) + return true; + } + + return false; +} + + +// manage phantoms +void CPhysicsEnvironment::PhantomAdd( CPhysicsObject *pObject ) +{ + IVP_Controller_Phantom *pPhantom = pObject->GetObject()->get_controller_phantom(); + if ( pPhantom ) + { + pPhantom->add_listener_phantom( m_pCollisionListener ); + } +} + +void CPhysicsEnvironment::PhantomRemove( CPhysicsObject *pObject ) +{ + IVP_Controller_Phantom *pPhantom = pObject->GetObject()->get_controller_phantom(); + if ( pPhantom ) + { + pPhantom->remove_listener_phantom( m_pCollisionListener ); + } +} + + +//------------------------------------- + +IPhysicsObject *CPhysicsEnvironment::CreateSphereObject( float radius, int materialIndex, const Vector& position, const QAngle& angles, objectparams_t *pParams, bool isStatic ) +{ + IPhysicsObject *pObject = ::CreatePhysicsSphere( this, radius, materialIndex, position, angles, pParams, isStatic ); + m_objects.AddToTail( pObject ); + return pObject; +} + +void CPhysicsEnvironment::TraceRay( const Ray_t &ray, unsigned int fMask, IPhysicsTraceFilter *pTraceFilter, trace_t *pTrace ) +{ +} + +void CPhysicsEnvironment::SweepCollideable( const CPhysCollide *pCollide, const Vector &vecAbsStart, const Vector &vecAbsEnd, + const QAngle &vecAngles, unsigned int fMask, IPhysicsTraceFilter *pTraceFilter, trace_t *pTrace ) +{ +} + + +void CPhysicsEnvironment::GetPerformanceSettings( physics_performanceparams_t *pOutput ) const +{ + if ( !pOutput ) + return; + + IVP_Anomaly_Limits *limits = m_pPhysEnv->get_anomaly_limits(); + if ( limits ) + { + // UNDONE: Expose these values for tuning + pOutput->maxVelocity = ConvertDistanceToHL( limits->max_velocity ); + pOutput->maxAngularVelocity = ConvertAngleToHL(limits->max_angular_velocity_per_psi) * m_pPhysEnv->get_inv_delta_PSI_time(); + pOutput->maxCollisionsPerObjectPerTimestep = limits->max_collisions_per_psi; + pOutput->maxCollisionChecksPerTimestep = limits->max_collision_checks_per_psi; + pOutput->minFrictionMass = limits->min_friction_mass; + pOutput->maxFrictionMass = limits->max_friction_mass; + } + + IVP_Range_Manager *range = m_pPhysEnv->get_range_manager(); + if ( range ) + { + pOutput->lookAheadTimeObjectsVsWorld = range->look_ahead_time_world; + pOutput->lookAheadTimeObjectsVsObject = range->look_ahead_time_intra; + } +} + +void CPhysicsEnvironment::SetPerformanceSettings( const physics_performanceparams_t *pSettings ) +{ + if ( !pSettings ) + return; + + IVP_Anomaly_Limits *limits = m_pPhysEnv->get_anomaly_limits(); + if ( limits ) + { + // UNDONE: Expose these values for tuning + limits->max_velocity = ConvertDistanceToIVP( pSettings->maxVelocity ); + limits->max_collisions_per_psi = pSettings->maxCollisionsPerObjectPerTimestep; + limits->max_collision_checks_per_psi = pSettings->maxCollisionChecksPerTimestep; + limits->max_angular_velocity_per_psi = ConvertAngleToIVP(pSettings->maxAngularVelocity) * m_pPhysEnv->get_delta_PSI_time(); + limits->min_friction_mass = clamp(pSettings->minFrictionMass, 1.0f, VPHYSICS_MAX_MASS ); + limits->max_friction_mass = clamp(pSettings->maxFrictionMass, 1.0f, VPHYSICS_MAX_MASS ); + } + + IVP_Range_Manager *range = m_pPhysEnv->get_range_manager(); + if ( range ) + { + range->look_ahead_time_world = pSettings->lookAheadTimeObjectsVsWorld; + range->look_ahead_time_intra = pSettings->lookAheadTimeObjectsVsObject; + } +} + + +// perf/cost statistics +void CPhysicsEnvironment::ReadStats( physics_stats_t *pOutput ) +{ + if ( !pOutput ) + return; + IVP_Statistic_Manager *stats = m_pPhysEnv->get_statistic_manager(); + if ( stats ) + { + pOutput->maxRescueSpeed = ConvertDistanceToHL( stats->max_rescue_speed ); + pOutput->maxSpeedGain = ConvertDistanceToHL( stats->max_speed_gain ); + pOutput->impactSysNum = stats->impact_sys_num; + pOutput->impactCounter = stats->impact_counter; + pOutput->impactSumSys = stats->impact_sum_sys; + pOutput->impactHardRescueCount = stats->impact_hard_rescue_counter; + pOutput->impactRescueAfterCount = stats->impact_rescue_after_counter; + + pOutput->impactDelayedCount = stats->impact_delayed_counter; + pOutput->impactCollisionChecks = stats->impact_coll_checks; + pOutput->impactStaticCount = stats->impact_unmov; + + pOutput->totalEnergyDestroyed = stats->sum_energy_destr; + pOutput->collisionPairsTotal = stats->sum_of_mindists; + pOutput->collisionPairsCreated = stats->mindists_generated; + pOutput->collisionPairsDestroyed = stats->mindists_deleted; + + pOutput->potentialCollisionsObjectVsObject = stats->range_intra_exceeded; + pOutput->potentialCollisionsObjectVsWorld = stats->range_world_exceeded; + + pOutput->frictionEventsProcessed = stats->processed_fmindists; + } +} + +void CPhysicsEnvironment::ClearStats() +{ + IVP_Statistic_Manager *stats = m_pPhysEnv->get_statistic_manager(); + if ( stats ) + { + stats->clear_statistic(); + } +} + +void CPhysicsEnvironment::EnableConstraintNotify( bool bEnable ) +{ + m_enableConstraintNotify = bEnable; +} + + +IPhysicsEnvironment *CreatePhysicsEnvironment( void ) +{ + return new CPhysicsEnvironment; +} + + +// This wraps IVP_Collision_Filter_Exclusive_Pair since we're reusing it +// as a general void * pair hash and it's API implies that has something +// to do with collision detection +class CVoidPairHash : private IVP_Collision_Filter_Exclusive_Pair +{ +public: + void AddPair( void *pObject0, void *pObject1 ) + { + // disabled pairs are stored int the collision filter's hash + disable_collision_between_objects( (IVP_Real_Object *)pObject0, (IVP_Real_Object *)pObject1 ); + } + + void RemovePair( void *pObject0, void *pObject1 ) + { + // enabling removes the stored hash pair + enable_collision_between_objects( (IVP_Real_Object *)pObject0, (IVP_Real_Object *)pObject1 ); + } + + bool HasPair( void *pObject0, void *pObject1 ) + { + // If collision is enabled, the pair is NOT present, so invert the test here. + return check_objects_for_collision_detection( (IVP_Real_Object *)pObject0, (IVP_Real_Object *)pObject1 ) ? false : true; + } +}; + + +class CObjectPairHash : public IPhysicsObjectPairHash +{ +public: + CObjectPairHash() + { + m_pObjectHash = new IVP_VHash_Store(1024); + } + + ~CObjectPairHash() + { + delete m_pObjectHash; + } + + // converts the void * stored in the hash to a list in the multilist + unsigned short HashToListIndex( void *pHash ) + { + if ( !pHash ) + return m_objectList.InvalidIndex(); + + unsigned int hash = (unsigned int)pHash; + // mask off the extra bit we added to avoid zeros + hash &= 0xFFFF; + return (unsigned short)hash; + } + + // converts the list in the multilist to a void * we can put in the hash + void *ListIndexToHash( unsigned short listIndex ) + { + unsigned int hash = (unsigned int)listIndex; + + // set the high bit, so zero means "not there" + hash |= 0x80000000; + return (void *)hash; + } + + // Lookup this object and get a multilist entry + unsigned short GetListForObject( void *pObject ) + { + return HashToListIndex( m_pObjectHash->find_elem( pObject ) ); + } + + // new object, set up his list + void SetListForObject( void *pObject, unsigned short listIndex ) + { + Assert( !m_pObjectHash->find_elem( pObject ) ); + m_pObjectHash->add_elem( pObject, ListIndexToHash(listIndex) ); + } + + // last entry is gone, remove the object + void DestroyListForObject( void *pObject, unsigned short listIndex ) + { + if ( m_objectList.IsValidList( listIndex ) ) + { + m_objectList.DestroyList( listIndex ); + m_pObjectHash->remove_elem( pObject ); + } + } + + // Add this object to the list of disabled objects + void AddToObjectList( void *pObject, void *pAdd ) + { + unsigned short listIndex = GetListForObject( pObject ); + if ( !m_objectList.IsValidList( listIndex ) ) + { + listIndex = m_objectList.CreateList(); + SetListForObject( pObject, listIndex ); + } + + m_objectList.AddToTail( listIndex, pAdd ); + } + + // Remove one object from a particular object's list (linear time) + void RemoveFromObjectList( void *pObject, void *pRemove ) + { + unsigned short listIndex = GetListForObject( pObject ); + if ( !m_objectList.IsValidList( listIndex ) ) + return; + + for ( unsigned short item = m_objectList.Head(listIndex); item != m_objectList.InvalidIndex(); item = m_objectList.Next(item) ) + { + if ( m_objectList[item] == pRemove ) + { + // found it, remove + m_objectList.Remove( listIndex, item ); + if ( m_objectList.Head(listIndex) == m_objectList.InvalidIndex() ) + { + DestroyListForObject( pObject, listIndex ); + } + return; + } + } + } + + // add a pair (constant time) + virtual void AddObjectPair( void *pObject0, void *pObject1 ) + { + if ( IsObjectPairInHash(pObject0,pObject1) ) + return; + + // add the pair to the hash + m_pairHash.AddPair( pObject0, pObject1 ); + AddToObjectList( pObject0, pObject1 ); + AddToObjectList( pObject1, pObject0 ); + } + + // remove a pair (linear time x 2) + virtual void RemoveObjectPair( void *pObject0, void *pObject1 ) + { + if ( !IsObjectPairInHash(pObject0,pObject1) ) + return; + + // remove the pair from the hash + m_pairHash.RemovePair( pObject0, pObject1 ); + RemoveFromObjectList( pObject0, pObject1 ); + RemoveFromObjectList( pObject1, pObject0 ); + } + + // check for pair presence (fast constant time) + virtual bool IsObjectPairInHash( void *pObject0, void *pObject1 ) + { + return m_pairHash.HasPair( pObject0, pObject1 ); + } + + virtual void RemoveAllPairsForObject( void *pObject ) + { + unsigned short listIndex = GetListForObject( pObject ); + if ( !m_objectList.IsValidList( listIndex ) ) + return; + + unsigned short item = m_objectList.Head(listIndex); + while ( item != m_objectList.InvalidIndex() ) + { + unsigned short next = m_objectList.Next(item); + void *pOther = m_objectList[item]; + m_objectList.Remove( listIndex, item ); + // remove the matching entry + RemoveFromObjectList( pOther, pObject ); + m_pairHash.RemovePair( pOther, pObject ); + item = next; + } + DestroyListForObject( pObject, listIndex ); + } + + // Gets the # of dependencies for a particular entity + virtual int GetPairCountForObject( void *pObject0 ) + { + unsigned short listIndex = GetListForObject( pObject0 ); + if ( !m_objectList.IsValidList( listIndex ) ) + return 0; + + int nCount = 0; + unsigned short item; + for ( item = m_objectList.Head(listIndex); item != m_objectList.InvalidIndex(); + item = m_objectList.Next(item) ) + { + ++nCount; + } + return nCount; + } + + // Gets all dependencies for a particular entity + virtual int GetPairListForObject( void *pObject0, int nMaxCount, void **ppObjectList ) + { + unsigned short listIndex = GetListForObject( pObject0 ); + if ( !m_objectList.IsValidList( listIndex ) ) + return 0; + + int nCount = 0; + unsigned short item; + for ( item = m_objectList.Head(listIndex); item != m_objectList.InvalidIndex(); + item = m_objectList.Next(item) ) + { + ppObjectList[nCount] = m_objectList[item]; + if ( ++nCount >= nMaxCount ) + break; + } + return nCount; + } + + virtual bool IsObjectInHash( void *pObject0 ) + { + return m_pObjectHash->find_elem(pObject0) != NULL ? true : false; + } +#if 0 + virtual int CountObjectsInHash() + { + return m_pObjectHash->n_elems(); + } +#endif + +private: + // this is a hash of object pairs + CVoidPairHash m_pairHash; + // this is a hash of pObjects with each element storing an index to the head of its list of disabled collisions + IVP_VHash_Store *m_pObjectHash; + + // this is the list of disabled collisions for each object. Uses multilist + CUtlMultiList<void *, unsigned short> m_objectList; +}; + +IPhysicsObjectPairHash *CreateObjectPairHash() +{ + return new CObjectPairHash; +} diff --git a/vphysics/physics_environment.h b/vphysics/physics_environment.h new file mode 100644 index 0000000..075b91c --- /dev/null +++ b/vphysics/physics_environment.h @@ -0,0 +1,176 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_WORLD_H +#define PHYSICS_WORLD_H +#pragma once + +#include "vphysics_interface.h" +#include "ivu_types.hxx" +#include "utlvector.h" + +class IVP_Environment; +class CSleepObjects; +class CPhysicsListenerCollision; +class CPhysicsListenerConstraint; +class IVP_Listener_Collision; +class IVP_Listener_Constraint; +class IVP_Listener_Object; +class IVP_Controller; +class CPhysicsFluidController; +class CCollisionSolver; +class CPhysicsObject; +class CDeleteQueue; +class IVPhysicsDebugOverlay; +struct constraint_limitedhingeparams_t; +struct vphysics_save_iphysicsobject_t; + +class CPhysicsEnvironment : public IPhysicsEnvironment +{ +public: + CPhysicsEnvironment( void ); + ~CPhysicsEnvironment( void ); + + virtual void SetDebugOverlay( CreateInterfaceFn debugOverlayFactory ); + virtual IVPhysicsDebugOverlay *GetDebugOverlay( void ); + + void SetGravity( const Vector& gravityVector ); + IPhysicsObject *CreatePolyObject( const CPhysCollide *pCollisionModel, int materialIndex, const Vector& position, const QAngle& angles, objectparams_t *pParams ); + IPhysicsObject *CreatePolyObjectStatic( const CPhysCollide *pCollisionModel, int materialIndex, const Vector& position, const QAngle& angles, objectparams_t *pParams ); + virtual unsigned int GetObjectSerializeSize( IPhysicsObject *pObject ) const; + virtual void SerializeObjectToBuffer( IPhysicsObject *pObject, unsigned char *pBuffer, unsigned int bufferSize ); + virtual IPhysicsObject *UnserializeObjectFromBuffer( void *pGameData, unsigned char *pBuffer, unsigned int bufferSize, bool enableCollisions ); + + + + IPhysicsSpring *CreateSpring( IPhysicsObject *pObjectStart, IPhysicsObject *pObjectEnd, springparams_t *pParams ); + IPhysicsFluidController *CreateFluidController( IPhysicsObject *pFluidObject, fluidparams_t *pParams ); + IPhysicsConstraint *CreateRagdollConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_ragdollparams_t &ragdoll ); + + virtual IPhysicsConstraint *CreateHingeConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_hingeparams_t &hinge ); + virtual IPhysicsConstraint *CreateLimitedHingeConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_limitedhingeparams_t &hinge ); + virtual IPhysicsConstraint *CreateFixedConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_fixedparams_t &fixed ); + virtual IPhysicsConstraint *CreateSlidingConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_slidingparams_t &sliding ); + virtual IPhysicsConstraint *CreateBallsocketConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_ballsocketparams_t &ballsocket ); + virtual IPhysicsConstraint *CreatePulleyConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_pulleyparams_t &pulley ); + virtual IPhysicsConstraint *CreateLengthConstraint( IPhysicsObject *pReferenceObject, IPhysicsObject *pAttachedObject, IPhysicsConstraintGroup *pGroup, const constraint_lengthparams_t &length ); + + virtual IPhysicsConstraintGroup *CreateConstraintGroup( const constraint_groupparams_t &group ); + virtual void DestroyConstraintGroup( IPhysicsConstraintGroup *pGroup ); + + void Simulate( float deltaTime ); + float GetSimulationTimestep() const; + void SetSimulationTimestep( float timestep ); + float GetSimulationTime() const; + float GetNextFrameTime() const; + bool IsInSimulation() const; + + virtual void DestroyObject( IPhysicsObject * ); + virtual void DestroySpring( IPhysicsSpring * ); + virtual void DestroyFluidController( IPhysicsFluidController * ); + virtual void DestroyConstraint( IPhysicsConstraint * ); + + virtual void SetCollisionEventHandler( IPhysicsCollisionEvent *pCollisionEvents ); + virtual void SetObjectEventHandler( IPhysicsObjectEvent *pObjectEvents ); + virtual void SetConstraintEventHandler( IPhysicsConstraintEvent *pConstraintEvents ); + + virtual IPhysicsShadowController *CreateShadowController( IPhysicsObject *pObject, bool allowTranslation, bool allowRotation ); + virtual void DestroyShadowController( IPhysicsShadowController * ); + virtual IPhysicsMotionController *CreateMotionController( IMotionEvent *pHandler ); + virtual void DestroyMotionController( IPhysicsMotionController *pController ); + virtual IPhysicsPlayerController *CreatePlayerController( IPhysicsObject *pObject ); + virtual void DestroyPlayerController( IPhysicsPlayerController *pController ); + virtual IPhysicsVehicleController *CreateVehicleController( IPhysicsObject *pVehicleBodyObject, const vehicleparams_t ¶ms, unsigned int nVehicleType, IPhysicsGameTrace *pGameTrace ); + virtual void DestroyVehicleController( IPhysicsVehicleController *pController ); + + virtual void SetQuickDelete( bool bQuick ) + { + m_deleteQuick = bQuick; + } + virtual bool ShouldQuickDelete() const { return m_deleteQuick; } + virtual void TraceBox( trace_t *ptr, const Vector &mins, const Vector &maxs, const Vector &start, const Vector &end ); + virtual void SetCollisionSolver( IPhysicsCollisionSolver *pCollisionSolver ); + virtual void GetGravity( Vector *pGravityVector ) const; + virtual int GetActiveObjectCount() const; + virtual void GetActiveObjects( IPhysicsObject **pOutputObjectList ) const; + virtual const IPhysicsObject **GetObjectList( int *pOutputObjectCount ) const; + virtual bool TransferObject( IPhysicsObject *pObject, IPhysicsEnvironment *pDestinationEnvironment ); + + IVP_Environment *GetIVPEnvironment( void ) { return m_pPhysEnv; } + void ClearDeadObjects( void ); + IVP_Controller *GetDragController() { return m_pDragController; } + const IVP_Controller *GetDragController() const { return m_pDragController; } + virtual void SetAirDensity( float density ); + virtual float GetAirDensity( void ) const; + virtual void ResetSimulationClock( void ); + virtual IPhysicsObject *CreateSphereObject( float radius, int materialIndex, const Vector &position, const QAngle &angles, objectparams_t *pParams, bool isStatic ); + virtual void CleanupDeleteList(); + virtual void EnableDeleteQueue( bool enable ) { m_queueDeleteObject = enable; } + // debug + virtual bool IsCollisionModelUsed( CPhysCollide *pCollide ) const; + + // trace against the physics world + virtual void TraceRay( const Ray_t &ray, unsigned int fMask, IPhysicsTraceFilter *pTraceFilter, trace_t *pTrace ); + virtual void SweepCollideable( const CPhysCollide *pCollide, const Vector &vecAbsStart, const Vector &vecAbsEnd, + const QAngle &vecAngles, unsigned int fMask, IPhysicsTraceFilter *pTraceFilter, trace_t *pTrace ); + + // performance tuning + virtual void GetPerformanceSettings( physics_performanceparams_t *pOutput ) const; + virtual void SetPerformanceSettings( const physics_performanceparams_t *pSettings ); + + // perf/cost statistics + virtual void ReadStats( physics_stats_t *pOutput ); + virtual void ClearStats(); + virtual void EnableConstraintNotify( bool bEnable ); + // debug + virtual void DebugCheckContacts(void); + + // Save/restore + bool Save( const physsaveparams_t ¶ms ); + void PreRestore( const physprerestoreparams_t ¶ms ); + bool Restore( const physrestoreparams_t ¶ms ); + void PostRestore(); + void PhantomAdd( CPhysicsObject *pObject ); + void PhantomRemove( CPhysicsObject *pObject ); + + void AddPlayerController( IPhysicsPlayerController *pController ); + void RemovePlayerController( IPhysicsPlayerController *pController ); + IPhysicsPlayerController *FindPlayerController( IPhysicsObject *pObject ); + + IPhysicsCollisionEvent *GetCollisionEventHandler(); + // a constraint is being disabled - report the game DLL as "broken" + void NotifyConstraintDisabled( IPhysicsConstraint *pConstraint ); + +private: + IVP_Environment *m_pPhysEnv; + IVP_Controller *m_pDragController; + IVPhysicsDebugOverlay *m_pDebugOverlay; // Interface to use for drawing debug overlays. + CUtlVector<IPhysicsObject *> m_objects; + CUtlVector<IPhysicsObject *> m_deadObjects; + CUtlVector<CPhysicsFluidController *> m_fluids; + CUtlVector<IPhysicsPlayerController *> m_playerControllers; + CSleepObjects *m_pSleepEvents; + CPhysicsListenerCollision *m_pCollisionListener; + CCollisionSolver *m_pCollisionSolver; + CPhysicsListenerConstraint *m_pConstraintListener; + CDeleteQueue *m_pDeleteQueue; + int m_lastObjectThisTick; + bool m_deleteQuick; + bool m_inSimulation; + bool m_queueDeleteObject; + bool m_fixedTimestep; + bool m_enableConstraintNotify; +}; + +extern IPhysicsEnvironment *CreatePhysicsEnvironment( void ); + +class IVP_Synapse_Friction; +class IVP_Real_Object; +extern IVP_Real_Object *GetOppositeSynapseObject( IVP_Synapse_Friction *pfriction ); +extern IPhysicsObjectPairHash *CreateObjectPairHash(); + +#endif // PHYSICS_WORLD_H diff --git a/vphysics/physics_fluid.cpp b/vphysics/physics_fluid.cpp new file mode 100644 index 0000000..8a8ec53 --- /dev/null +++ b/vphysics/physics_fluid.cpp @@ -0,0 +1,231 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" +#include "physics_fluid.h" + + +#include "ivp_compact_surface.hxx" +#include "ivp_surman_polygon.hxx" +#include "ivp_phantom.hxx" +#include "ivp_controller_buoyancy.hxx" +#include "ivp_liquid_surface_descript.hxx" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +// NOTE: This is auto-deleted by the phantom controller +class CBuoyancyAttacher : public IVP_Attacher_To_Cores_Buoyancy +{ +public: + virtual IVP_Template_Buoyancy *get_parameters_per_core( IVP_Core *pCore ); + CBuoyancyAttacher(IVP_Template_Buoyancy &templ, IVP_U_Set_Active<IVP_Core> *set_of_cores_, IVP_Liquid_Surface_Descriptor *liquid_surface_descriptor_); + + float m_density; +}; + +CPhysicsFluidController::CPhysicsFluidController( CBuoyancyAttacher *pBuoy, IVP_Liquid_Surface_Descriptor *pLiquid, CPhysicsObject *pObject, int nContents ) +{ + m_pBuoyancy = pBuoy; + m_pLiquidSurface = pLiquid; + m_pObject = pObject; + m_nContents = nContents; +} + +CPhysicsFluidController::~CPhysicsFluidController( void ) +{ + delete m_pLiquidSurface; +} + +void CPhysicsFluidController::SetGameData( void *pGameData ) +{ + m_pGameData = pGameData; +} + +void *CPhysicsFluidController::GetGameData( void ) const +{ + return m_pGameData; +} + +void CPhysicsFluidController::GetSurfacePlane( Vector *pNormal, float *pDist ) const +{ + IVP_U_Float_Hesse surface; + IVP_U_Float_Point abs_speed_of_current; + + m_pLiquidSurface->calc_liquid_surface( GetIVPObject()->get_core()->environment, + GetIVPObject()->get_core(), &surface, &abs_speed_of_current ); + ConvertPlaneToHL( surface, pNormal, pDist ); + if ( pNormal ) + { + *pNormal *= -1; + } + if ( pDist ) + { + *pDist *= -1; + } +} + +IVP_Real_Object *CPhysicsFluidController::GetIVPObject() +{ + return m_pObject->GetObject(); +} + +const IVP_Real_Object *CPhysicsFluidController::GetIVPObject() const +{ + return m_pObject->GetObject(); +} + +float CPhysicsFluidController::GetDensity() const +{ + return m_pBuoyancy->m_density; +} + +void CPhysicsFluidController::WakeAllSleepingObjects() +{ + GetIVPObject()->get_controller_phantom()->wake_all_sleeping_objects(); +} + +int CPhysicsFluidController::GetContents() const +{ + return m_nContents; +} + +IVP_Template_Buoyancy *CBuoyancyAttacher::get_parameters_per_core( IVP_Core *pCore ) +{ + if ( pCore ) + { + IVP_Real_Object *pivp = pCore->objects.element_at(0); + CPhysicsObject *pPhys = static_cast<CPhysicsObject *>(pivp->client_data); + + // This ratio is for objects whose mass / (collision model) volume is not equal to their density. + // Keep the fluid pressure/friction solution for the volume, but scale the buoyant force calculations + // to be in line with the object's real density. This is accompilshed by changing the fluid's density + // on a per-object basis. + float ratio = pPhys->GetBuoyancyRatio(); + + if ( pPhys->GetShadowController() || !(pPhys->CallbackFlags() & CALLBACK_DO_FLUID_SIMULATION) ) + { + // NOTE: don't do buoyancy on these guys for now! + template_buoyancy.medium_density = 0; + } + else + { + template_buoyancy.medium_density = m_density * ratio; + } + } + else + { + template_buoyancy.medium_density = m_density; + } + + return &template_buoyancy; +} + +CBuoyancyAttacher::CBuoyancyAttacher(IVP_Template_Buoyancy &templ, IVP_U_Set_Active<IVP_Core> *set_of_cores_, IVP_Liquid_Surface_Descriptor *liquid_surface_descriptor_) + :IVP_Attacher_To_Cores_Buoyancy(templ, set_of_cores_, liquid_surface_descriptor_) +{ + m_density = templ.medium_density; +} + + +//----------------------------------------------------------------------------- +// Defines the surface descriptor in local space +//----------------------------------------------------------------------------- +class CLiquidSurfaceDescriptor : public IVP_Liquid_Surface_Descriptor +{ +public: + CLiquidSurfaceDescriptor( CPhysicsObject *pFluidObject, const Vector4D &plane, const Vector ¤t ) + { + cplane_t worldPlane; + worldPlane.normal = plane.AsVector3D(); + worldPlane.dist = plane[3]; + + matrix3x4_t matObjectToWorld; + pFluidObject->GetPositionMatrix( &matObjectToWorld ); + MatrixITransformPlane( matObjectToWorld, worldPlane, m_objectSpacePlane ); + + VectorIRotate( current, matObjectToWorld, m_vecObjectSpaceCurrent ); + m_pFluidObject = pFluidObject; + } + + virtual void calc_liquid_surface( IVP_Environment * /*environment*/, + IVP_Core * /*core*/, + IVP_U_Float_Hesse *surface_normal_out, + IVP_U_Float_Point *abs_speed_of_current_out) + { + cplane_t worldPlane; + matrix3x4_t matObjectToWorld; + m_pFluidObject->GetPositionMatrix( &matObjectToWorld ); + MatrixTransformPlane( matObjectToWorld, m_objectSpacePlane, worldPlane ); + + worldPlane.normal *= -1.0f; + worldPlane.dist *= -1.0f; + + IVP_U_Float_Hesse worldSurface; + ConvertPlaneToIVP( worldPlane.normal, worldPlane.dist, worldSurface ); + surface_normal_out->set(&worldSurface); + surface_normal_out->hesse_val = worldSurface.hesse_val; + + Vector worldSpaceCurrent; + VectorRotate( m_vecObjectSpaceCurrent, matObjectToWorld, worldSpaceCurrent ); + + IVP_U_Float_Point ivpWorldSpaceCurrent; + ConvertDirectionToIVP( worldSpaceCurrent, ivpWorldSpaceCurrent ); + abs_speed_of_current_out->set( &ivpWorldSpaceCurrent ); + } + +private: + Vector m_vecObjectSpaceCurrent; + cplane_t m_objectSpacePlane; + CPhysicsObject *m_pFluidObject; +}; + + +CPhysicsFluidController *CreateFluidController( IVP_Environment *pEnvironment, CPhysicsObject *pFluidObject, fluidparams_t *pParams ) +{ + pFluidObject->BecomeTrigger(); + + IVP_Controller_Phantom *pPhantom = pFluidObject->GetObject()->get_controller_phantom(); + if ( !pPhantom ) + return NULL; + + IVP_Liquid_Surface_Descriptor *lsd = new CLiquidSurfaceDescriptor( pFluidObject, pParams->surfacePlane, pParams->currentVelocity ); + int surfaceprops = pFluidObject->GetMaterialIndex(); + float density = physprops->GetSurfaceData( surfaceprops )->physics.density; + // --------------------------------------------- + // create parameter template for Buoyancy_Solver + // --------------------------------------------- + // UNDONE: Expose these other parametersd + IVP_Template_Buoyancy buoyancy_input; + buoyancy_input.medium_density = ConvertDensityToIVP(density); // density of water (unit: kg/m^3) + buoyancy_input.pressure_damp_factor = pParams->damping; + buoyancy_input.viscosity_factor = 0.0f; + buoyancy_input.torque_factor = 0.01f; + buoyancy_input.viscosity_input_factor = 0.1f; + // ------------------------------------------------------------------------------- + // create "water" (i.e. buoyancy solver) and attach a dynamic list of object cores + // ------------------------------------------------------------------------------- + CBuoyancyAttacher *attacher_to_cores_buoyancy = new CBuoyancyAttacher( buoyancy_input, pPhantom->get_intruding_cores(), lsd ); + + CPhysicsFluidController *pFluid = new CPhysicsFluidController( attacher_to_cores_buoyancy, lsd, pFluidObject, pParams->contents ); + pFluid->SetGameData( pParams->pGameData ); + pPhantom->client_data = static_cast<void *>(pFluid); + + return pFluid; +} + + +bool SavePhysicsFluidController( const physsaveparams_t ¶ms, CPhysicsFluidController *pFluidObject ) +{ + return false; +} + +bool RestorePhysicsFluidController( const physrestoreparams_t ¶ms, CPhysicsFluidController **ppFluidObject ) +{ + return false; +} + diff --git a/vphysics/physics_fluid.h b/vphysics/physics_fluid.h new file mode 100644 index 0000000..6aa475f --- /dev/null +++ b/vphysics/physics_fluid.h @@ -0,0 +1,49 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_FLUID_H +#define PHYSICS_FLUID_H +#pragma once + +#include "vphysics_interface.h" + +class IVP_Compact_Surface; +class IVP_Environment; +class IVP_Listener_Phantom; +class CBuoyancyAttacher; +class IVP_Liquid_Surface_Descriptor; +class CPhysicsObject; +class CPhysicsObject; + +class CPhysicsFluidController : public IPhysicsFluidController +{ +public: + CPhysicsFluidController( CBuoyancyAttacher *pBuoy, IVP_Liquid_Surface_Descriptor *pLiquid, CPhysicsObject *pObject, int nContents ); + ~CPhysicsFluidController( void ); + + virtual void SetGameData( void *pGameData ); + virtual void *GetGameData( void ) const; + virtual void GetSurfacePlane( Vector *pNormal, float *pDist ) const; + virtual float GetDensity() const; + virtual void WakeAllSleepingObjects(); + virtual int GetContents() const; + + class IVP_Real_Object *GetIVPObject(); + const class IVP_Real_Object *GetIVPObject() const; + +private: + CBuoyancyAttacher *m_pBuoyancy; + IVP_Liquid_Surface_Descriptor *m_pLiquidSurface; + CPhysicsObject *m_pObject; + int m_nContents; + void *m_pGameData; +}; + +extern CPhysicsFluidController *CreateFluidController( IVP_Environment *pEnvironment, CPhysicsObject *pFluidObject, fluidparams_t *pParams ); + + +#endif // PHYSICS_FLUID_H diff --git a/vphysics/physics_friction.cpp b/vphysics/physics_friction.cpp new file mode 100644 index 0000000..a1afa52 --- /dev/null +++ b/vphysics/physics_friction.cpp @@ -0,0 +1,198 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//=============================================================================// + +#include "cbase.h" + +#include "physics_friction.h" +#include "vphysics/friction.h" + +#include "ivp_mindist.hxx" +#include "ivp_listener_collision.hxx" +#include "ivp_friction.hxx" + + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +class CFrictionSnapshot : public IPhysicsFrictionSnapshot +{ +public: + CFrictionSnapshot( IVP_Real_Object *pObject ); + ~CFrictionSnapshot(); + + bool IsValid(); + + // Object 0 is this object, Object 1 is the other object + IPhysicsObject *GetObject( int index ); + int GetMaterial( int index ); + + void GetContactPoint( Vector &out ); + void GetSurfaceNormal( Vector &out ); + float GetNormalForce(); + float GetEnergyAbsorbed(); + void RecomputeFriction(); + void ClearFrictionForce(); + + void MarkContactForDelete(); + void DeleteAllMarkedContacts( bool wakeObjects ); + void NextFrictionData(); + float GetFrictionCoefficient(); + + +private: + void SetFrictionSynapse( IVP_Synapse_Friction *pSet ); + CUtlVector<IVP_Real_Object *> *m_pDeleteList; + IVP_Real_Object *m_pObject; + IVP_Synapse_Friction *m_pFriction; + IVP_Contact_Point *m_pContactPoint; + int m_synapseIndex; +}; + +CFrictionSnapshot::CFrictionSnapshot( IVP_Real_Object *pObject ) : m_pObject(pObject) +{ + m_pDeleteList = NULL; + SetFrictionSynapse( pObject->get_first_friction_synapse() ); +} + +CFrictionSnapshot::~CFrictionSnapshot() +{ + delete m_pDeleteList; +} + +void CFrictionSnapshot::DeleteAllMarkedContacts( bool wakeObjects ) +{ + if ( !m_pDeleteList ) + return; + + for ( int i = 0; i < m_pDeleteList->Count(); i++ ) + { + if ( wakeObjects ) + { + m_pDeleteList->Element(i)->ensure_in_simulation(); + } + DeleteAllFrictionPairs( m_pObject, m_pDeleteList->Element(i) ); + } + m_pFriction = NULL; +} + +void CFrictionSnapshot::SetFrictionSynapse( IVP_Synapse_Friction *pSet ) +{ + if ( pSet ) + { + m_pFriction = pSet; + m_pContactPoint = pSet->get_contact_point(); + m_synapseIndex = (pSet == m_pContactPoint->get_synapse(0)) ? 0 : 1; + } + else + { + m_pFriction = NULL; + m_pContactPoint = NULL; + m_synapseIndex = 0; + } +} + +bool CFrictionSnapshot::IsValid() +{ + return m_pFriction != NULL ? true : false; +} + +IPhysicsObject *CFrictionSnapshot::GetObject( int index ) +{ + IVP_Synapse_Friction *pFriction = m_pFriction; + if ( index == 1 ) + { + pFriction = m_pContactPoint->get_synapse(!m_synapseIndex); + } + return static_cast<IPhysicsObject *>(pFriction->get_object()->client_data); +} + +void CFrictionSnapshot::MarkContactForDelete() +{ + IVP_Synapse_Friction *pFriction = m_pContactPoint->get_synapse(!m_synapseIndex); + IVP_Real_Object *pObject = pFriction->get_object(); + Assert(pObject != m_pObject); + if ( pObject != m_pObject ) + { + if ( !m_pDeleteList ) + { + m_pDeleteList = new CUtlVector<IVP_Real_Object *>; + } + m_pDeleteList->AddToTail( pObject ); + } +} + +int CFrictionSnapshot::GetMaterial( int index ) +{ + IVP_Material *ivpMats[2]; + + m_pContactPoint->get_material_info(ivpMats); + + // index 1 is the other one + index ^= m_synapseIndex; + + return physprops->GetIVPMaterialIndex( ivpMats[index] ); +} + +void CFrictionSnapshot::GetContactPoint( Vector &out ) +{ + ConvertPositionToHL( *m_pContactPoint->get_contact_point_ws(), out ); +} + +void CFrictionSnapshot::GetSurfaceNormal( Vector &out ) +{ + float sign[2] = {1,-1}; + IVP_U_Float_Point normal; + IVP_Contact_Point_API::get_surface_normal_ws(const_cast<IVP_Contact_Point *>(m_pContactPoint), &normal ); + ConvertDirectionToHL( normal, out ); + out *= sign[m_synapseIndex]; + VectorNormalize(out); +} + +float CFrictionSnapshot::GetFrictionCoefficient() +{ + return m_pContactPoint->get_friction_factor(); +} + +float CFrictionSnapshot::GetNormalForce() +{ + return ConvertDistanceToHL( IVP_Contact_Point_API::get_vert_force( m_pContactPoint ) ); +} + +float CFrictionSnapshot::GetEnergyAbsorbed() +{ + return ConvertEnergyToHL( IVP_Contact_Point_API::get_eliminated_energy( m_pContactPoint ) ); +} + +void CFrictionSnapshot::RecomputeFriction() +{ + m_pContactPoint->recompute_friction(); +} + +void CFrictionSnapshot::ClearFrictionForce() +{ + m_pContactPoint->set_friction_to_neutral(); +} + +void CFrictionSnapshot::NextFrictionData() +{ + SetFrictionSynapse( m_pFriction->get_next() ); +} + +IPhysicsFrictionSnapshot *CreateFrictionSnapshot( IVP_Real_Object *pObject ) +{ + return new CFrictionSnapshot( pObject ); +} + +void DestroyFrictionSnapshot( IPhysicsFrictionSnapshot *pSnapshot ) +{ + delete pSnapshot; +} + + +void DeleteAllFrictionPairs( IVP_Real_Object *pObject0, IVP_Real_Object *pObject1 ) +{ + pObject0->unlink_contact_points_for_object( pObject1 ); +} diff --git a/vphysics/physics_friction.h b/vphysics/physics_friction.h new file mode 100644 index 0000000..9314d84 --- /dev/null +++ b/vphysics/physics_friction.h @@ -0,0 +1,21 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//=============================================================================// + +#ifndef PHYSICS_FRICTION_H +#define PHYSICS_FRICTION_H +#ifdef _WIN32 +#pragma once +#endif + + +class IVP_Real_Object; +class IPhysicsFrictionSnapshot; + +IPhysicsFrictionSnapshot *CreateFrictionSnapshot( IVP_Real_Object *pObject ); +void DestroyFrictionSnapshot( IPhysicsFrictionSnapshot *pSnapshot ); +void DeleteAllFrictionPairs( IVP_Real_Object *pObject0, IVP_Real_Object *pObject1 ); + +#endif // PHYSICS_FRICTION_H diff --git a/vphysics/physics_material.cpp b/vphysics/physics_material.cpp new file mode 100644 index 0000000..3073217 --- /dev/null +++ b/vphysics/physics_material.cpp @@ -0,0 +1,643 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" +#include "physics_vehicle.h" + +#include "ivp_material.hxx" +#include <ctype.h> +#include "utlsymbol.h" +#include "tier1/strtools.h" +#include "vcollide_parse_private.h" +#include "ctype.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +//----------------------------------------------------------------------------- +// Purpose: This is the data stored for each material/surface propery list +//----------------------------------------------------------------------------- +class CSurface : public IVP_Material +{ +public: + + // IVP_Material + virtual IVP_DOUBLE get_friction_factor() + { + return data.physics.friction; + } + + virtual IVP_DOUBLE get_elasticity() + { + return data.physics.elasticity; + } + virtual const char *get_name(); + // UNDONE: not implemented here. + virtual IVP_DOUBLE get_second_friction_factor() + { + return 0; + } + virtual IVP_DOUBLE get_adhesion() + { + return 0; + } + + virtual IVP_DOUBLE get_damping() + { + return data.physics.dampening; + } + + // strings + CUtlSymbol m_name; + unsigned short m_pad; + + // physics properties + surfacedata_t data; +}; + + +class CPhysicsSurfaceProps; + +class CIVPMaterialManager : public IVP_Material_Manager +{ + typedef IVP_Material_Manager BaseClass; +public: + CIVPMaterialManager( void ); + void Init( CPhysicsSurfaceProps *pProps ) { m_props = pProps; } + void SetPropMap( int *map, int mapSize ); + int RemapIVPMaterialIndex( int ivpMaterialIndex ) const; + + // IVP_Material_Manager + virtual IVP_Material *get_material_by_index(IVP_Real_Object *pObject, const IVP_U_Point *world_position, int index); + + virtual IVP_DOUBLE get_friction_factor(IVP_Contact_Situation *situation) // returns values >0, value of 1.0f means object stands on a 45 degres hill + { + // vehicle wheels get no friction with stuff that isn't ground + // helps keep control of the car + // traction on less than 60 degree slopes. + float wheelFriction = 1.0f; + if ( ShouldOverrideWheelContactFriction( &wheelFriction, situation->objects[0], situation->objects[1], &situation->surf_normal ) ) + { + return wheelFriction; + } + + IVP_DOUBLE factor = BaseClass::get_friction_factor( situation ); + factor = clamp(factor,0.0,1.0); + + return factor; + } + + virtual IVP_DOUBLE get_elasticity(IVP_Contact_Situation *situation) // range [0, 1.0f[, the relative speed after a collision compared to the speed before + { + IVP_DOUBLE flElasticity = BaseClass::get_elasticity( situation ); + if ( flElasticity > 1.0f ) + { + flElasticity = 1.0f; + } + else if ( flElasticity < 0 ) + { + flElasticity = 0; + } + return flElasticity; + } + +private: + CPhysicsSurfaceProps *m_props; + unsigned short m_propMap[128]; +}; + + +//----------------------------------------------------------------------------- +// Purpose: This is the main database of materials +//----------------------------------------------------------------------------- +class CPhysicsSurfaceProps : public IPhysicsSurfacePropsInternal +{ +public: + CPhysicsSurfaceProps( void ); + ~CPhysicsSurfaceProps( void ); + + virtual int ParseSurfaceData( const char *pFilename, const char *pTextfile ); + virtual int SurfacePropCount( void ) const; + virtual int GetSurfaceIndex( const char *pPropertyName ) const; + virtual void GetPhysicsProperties( int surfaceDataIndex, float *density, float *thickness, float *friction, float *elasticity ) const; + virtual void GetPhysicsParameters( int surfaceDataIndex, surfacephysicsparams_t *pParamsOut ) const; + virtual surfacedata_t *GetSurfaceData( int surfaceDataIndex ); + virtual const char *GetString( unsigned short stringTableIndex ) const; + virtual const char *GetPropName( int surfaceDataIndex ) const; + virtual void SetWorldMaterialIndexTable( int *pMapArray, int mapSize ); + virtual int RemapIVPMaterialIndex( int ivpMaterialIndex ) const + { + return m_ivpManager.RemapIVPMaterialIndex( ivpMaterialIndex ); + } + bool IsReservedMaterialIndex( int materialIndex ) const; + virtual const char *GetReservedMaterialName( int materialIndex ) const; + int GetReservedFallBack( int materialIndex ) const; + + int GetReservedSurfaceIndex( const char *pPropertyName ) const; + + // The database is derived from the IVP material class + const IVP_Material *GetIVPMaterial( int materialIndex ) const; + IVP_Material *GetIVPMaterial( int materialIndex ); + virtual int GetIVPMaterialIndex( const IVP_Material *pIVP ) const; + IVP_Material_Manager *GetIVPManager( void ) { return &m_ivpManager; } + + const char *GetNameString( CUtlSymbol name ) const + { + return m_strings.String(name); + } + +private: + const CSurface *GetInternalSurface( int materialIndex ) const; + CSurface *GetInternalSurface( int materialIndex ); + + void CopyPhysicsProperties( CSurface *pOut, int baseIndex ); + bool AddFileToDatabase( const char *pFilename ); + +private: + CUtlSymbolTableMT m_strings; + CUtlVector<CSurface> m_props; + CUtlVector<CUtlSymbol> m_fileList; + CIVPMaterialManager m_ivpManager; + bool m_init; + int m_shadowFallback; +}; + + +// Singleton database object +CPhysicsSurfaceProps g_SurfaceDatabase; +EXPOSE_SINGLE_INTERFACE_GLOBALVAR(CPhysicsSurfaceProps, IPhysicsSurfaceProps, VPHYSICS_SURFACEPROPS_INTERFACE_VERSION, g_SurfaceDatabase); + + +// Global pointer to singleton for VPHYSICS.DLL internal access +IPhysicsSurfacePropsInternal *physprops = &g_SurfaceDatabase; + + +const char *CSurface::get_name() +{ + return g_SurfaceDatabase.GetNameString( m_name ); +} + +CPhysicsSurfaceProps::CPhysicsSurfaceProps( void ) : m_fileList(8,8), m_strings( 0, 32, true ) +{ + m_ivpManager.Init( this ); + // Force index 0 to be the empty string. Allows game code to check for zero, but + // still resolve to a string + m_strings.AddString(""); + m_init = false; + m_shadowFallback = 0; +} + + +CPhysicsSurfaceProps::~CPhysicsSurfaceProps( void ) +{ +} + +int CPhysicsSurfaceProps::SurfacePropCount( void ) const +{ + return m_props.Size(); +} + +// Add the filename to a list to make sure each file is only processed once +bool CPhysicsSurfaceProps::AddFileToDatabase( const char *pFilename ) +{ + CUtlSymbol id = m_strings.AddString( pFilename ); + + for ( int i = 0; i < m_fileList.Size(); i++ ) + { + if ( m_fileList[i] == id ) + return false; + } + + m_fileList.AddToTail( id ); + return true; +} + +int CPhysicsSurfaceProps::GetSurfaceIndex( const char *pPropertyName ) const +{ + if ( pPropertyName[0] == '$' ) + { + int index = GetReservedSurfaceIndex( pPropertyName ); + if ( index >= 0 ) + return index; + } + + CUtlSymbol id = m_strings.Find( pPropertyName ); + if ( id.IsValid() ) + { + // BUGBUG: Linear search is slow!!! + for ( int i = 0; i < m_props.Size(); i++ ) + { + // NOTE: Just comparing strings by index is pretty fast though + if ( m_props[i].m_name == id ) + return i; + } + } + + return -1; +} + + +const char *CPhysicsSurfaceProps::GetPropName( int surfaceDataIndex ) const +{ + const CSurface *pSurface = GetInternalSurface( surfaceDataIndex ); + if ( pSurface ) + { + return GetNameString( pSurface->m_name ); + } + return NULL; +} + + +// UNDONE: move reserved materials into this table, or into a parallel table +// that gets hooked out here. +CSurface *CPhysicsSurfaceProps::GetInternalSurface( int materialIndex ) +{ + if ( IsReservedMaterialIndex( materialIndex ) ) + { + materialIndex = GetReservedFallBack( materialIndex ); + } + if ( materialIndex < 0 || materialIndex > m_props.Size()-1 ) + { + return NULL; + } + return &m_props[materialIndex]; +} + +// this function is actually const except for the return type, so this is safe +const CSurface *CPhysicsSurfaceProps::GetInternalSurface( int materialIndex ) const +{ + return const_cast<CPhysicsSurfaceProps *>(this)->GetInternalSurface(materialIndex); +} + +void CPhysicsSurfaceProps::GetPhysicsProperties( int materialIndex, float *density, float *thickness, float *friction, float *elasticity ) const +{ + const CSurface *pSurface = GetInternalSurface( materialIndex ); + if ( !pSurface ) + { + pSurface = GetInternalSurface( GetSurfaceIndex( "default" ) ); + Assert ( pSurface ); + } + if ( pSurface ) + { + if ( friction ) + { + *friction = (float)pSurface->data.physics.friction; + } + if ( elasticity ) + { + *elasticity = (float)pSurface->data.physics.elasticity; + } + if ( density ) + { + *density = pSurface->data.physics.density; + } + if ( thickness ) + { + *thickness = pSurface->data.physics.thickness; + } + } +} + +void CPhysicsSurfaceProps::GetPhysicsParameters( int surfaceDataIndex, surfacephysicsparams_t *pParamsOut ) const +{ + if ( !pParamsOut ) + return; + + const CSurface *pSurface = GetInternalSurface( surfaceDataIndex ); + if ( pSurface ) + { + *pParamsOut = pSurface->data.physics; + } +} + +surfacedata_t *CPhysicsSurfaceProps::GetSurfaceData( int materialIndex ) +{ + CSurface *pSurface = GetInternalSurface( materialIndex ); + if (!pSurface) + pSurface = GetInternalSurface( 0 ); // Zero is always the "default" property + + Assert ( pSurface ); + return &pSurface->data; +} + +const char *CPhysicsSurfaceProps::GetString( unsigned short stringTableIndex ) const +{ + return m_strings.String( stringTableIndex ); +} + + +bool CPhysicsSurfaceProps::IsReservedMaterialIndex( int materialIndex ) const +{ + return (materialIndex > 127) ? true : false; +} + +const char *CPhysicsSurfaceProps::GetReservedMaterialName( int materialIndex ) const +{ + // NOTE: All of these must start with '$' + switch( materialIndex ) + { + case MATERIAL_INDEX_SHADOW: + return "$MATERIAL_INDEX_SHADOW"; + } + + return NULL; +} + +int CPhysicsSurfaceProps::GetReservedSurfaceIndex( const char *pPropertyName ) const +{ + if ( !Q_stricmp( pPropertyName, "$MATERIAL_INDEX_SHADOW" ) ) + { + return MATERIAL_INDEX_SHADOW; + } + return -1; +} + +const IVP_Material *CPhysicsSurfaceProps::GetIVPMaterial( int materialIndex ) const +{ + return GetInternalSurface(materialIndex); +} + +IVP_Material *CPhysicsSurfaceProps::GetIVPMaterial( int materialIndex ) +{ + return GetInternalSurface(materialIndex); +} + + +int CPhysicsSurfaceProps::GetReservedFallBack( int materialIndex ) const +{ + switch( materialIndex ) + { + case MATERIAL_INDEX_SHADOW: + return m_shadowFallback; + } + + return 0; +} + + +int CPhysicsSurfaceProps::GetIVPMaterialIndex( const IVP_Material *pIVP ) const +{ + int index = (const CSurface *)pIVP - m_props.Base(); + if ( index >= 0 && index < m_props.Size() ) + return index; + + return -1; +} + + +void CPhysicsSurfaceProps::CopyPhysicsProperties( CSurface *pOut, int baseIndex ) +{ + const CSurface *pSurface = GetInternalSurface( baseIndex ); + if ( pSurface ) + { + pOut->data = pSurface->data; + } +} + + +int CPhysicsSurfaceProps::ParseSurfaceData( const char *pFileName, const char *pTextfile ) +{ + if ( !AddFileToDatabase( pFileName ) ) + { + return 0; + } + + const char *pText = pTextfile; + + do + { + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + pText = ParseKeyvalue( pText, key, value ); + if ( !strcmp(value, "{") ) + { + CSurface prop; + memset( &prop.data, 0, sizeof(prop.data) ); + prop.m_name = m_strings.AddString( key ); + int baseMaterial = GetSurfaceIndex( key ); + if ( baseMaterial < 0 ) + { + baseMaterial = GetSurfaceIndex( "default" ); + } + + CopyPhysicsProperties( &prop, baseMaterial ); + + do + { + pText = ParseKeyvalue( pText, key, value ); + if ( !strcmpi( key, "}" ) ) + { + // already in the database, don't add again, override values instead + const char *pOverride = m_strings.String(prop.m_name); + int propIndex = GetSurfaceIndex( pOverride ); + if ( propIndex >= 0 ) + { + CSurface *pSurface = GetInternalSurface( propIndex ); + pSurface->data = prop.data; + break; + } + + m_props.AddToTail( prop ); + break; + } + else if ( !strcmpi( key, "base" ) ) + { + baseMaterial = GetSurfaceIndex( value ); + CopyPhysicsProperties( &prop, baseMaterial ); + } + else if ( !strcmpi( key, "thickness" ) ) + { + prop.data.physics.thickness = atof(value); + } + else if ( !strcmpi( key, "density" ) ) + { + prop.data.physics.density = atof(value); + } + else if ( !strcmpi( key, "elasticity" ) ) + { + prop.data.physics.elasticity = atof(value); + } + else if ( !strcmpi( key, "friction" ) ) + { + prop.data.physics.friction = atof(value); + } + else if ( !strcmpi( key, "maxspeedfactor" ) ) + { + prop.data.game.maxSpeedFactor = atof(value); + } + else if ( !strcmpi( key, "jumpfactor" ) ) + { + prop.data.game.jumpFactor = atof(value); + } + else if ( !strcmpi( key, "climbable" ) ) + { + prop.data.game.climbable = atoi(value); + } + // audio parameters + else if ( !strcmpi( key, "audioReflectivity" ) ) + { + prop.data.audio.reflectivity = atof(value); + } + else if ( !strcmpi( key, "audioHardnessFactor" ) ) + { + prop.data.audio.hardnessFactor = atof(value); + } + else if ( !strcmpi( key, "audioHardMinVelocity" ) ) + { + prop.data.audio.hardVelocityThreshold = atof(value); + } + else if ( !strcmpi( key, "audioRoughnessFactor" ) ) + { + prop.data.audio.roughnessFactor = atof(value); + } + else if ( !strcmpi( key, "scrapeRoughThreshold" ) ) + { + prop.data.audio.roughThreshold = atof(value); + } + else if ( !strcmpi( key, "impactHardThreshold" ) ) + { + prop.data.audio.hardThreshold = atof(value); + } + // sound names + else if ( !strcmpi( key, "stepleft" ) ) + { + prop.data.sounds.stepleft = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "stepright" ) ) + { + prop.data.sounds.stepright = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "impactsoft" ) ) + { + prop.data.sounds.impactSoft = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "impacthard" ) ) + { + prop.data.sounds.impactHard = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "scrapesmooth" ) ) + { + prop.data.sounds.scrapeSmooth = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "scraperough" ) ) + { + prop.data.sounds.scrapeRough = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "bulletimpact" ) ) + { + prop.data.sounds.bulletImpact = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "break" ) ) + { + prop.data.sounds.breakSound = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "strain" ) ) + { + prop.data.sounds.strainSound = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "rolling" ) ) + { + prop.data.sounds.rolling = m_strings.AddString( value ); + } + else if ( !strcmpi( key, "gamematerial" ) ) + { + if ( strlen(value) == 1 && !V_isdigit( value[0]) ) + { + prop.data.game.material = toupper(value[0]); + } + else + { + prop.data.game.material = atoi(value); + } + } + else if ( !strcmpi( key, "dampening" ) ) + { + prop.data.physics.dampening = atof(value); + } + else + { + // force a breakpoint + AssertMsg2( 0, "Bad surfaceprop key %s (%s)\n", key, value ); + } + } while (pText); + } + } while (pText); + + if ( !m_init ) + { + m_init = true; + //AddReservedMaterials + CSurface prop; + + int baseMaterial = GetSurfaceIndex( "default" ); + memset( &prop.data, 0, sizeof(prop.data) ); + prop.m_name = m_strings.AddString( GetReservedMaterialName(MATERIAL_INDEX_SHADOW) ); + CopyPhysicsProperties( &prop, baseMaterial ); + prop.data.physics.elasticity = 1e-3f; + prop.data.physics.friction = 0.8f; + m_shadowFallback = m_props.AddToTail( prop ); + } + return m_props.Size(); +} + + +void CPhysicsSurfaceProps::SetWorldMaterialIndexTable( int *pMapArray, int mapSize ) +{ + m_ivpManager.SetPropMap( pMapArray, mapSize ); +} + +CIVPMaterialManager::CIVPMaterialManager( void ) : IVP_Material_Manager( IVP_FALSE ) +{ + // by default every index maps to itself (NULL translation) + for ( int i = 0; i < ARRAYSIZE(m_propMap); i++ ) + { + m_propMap[i] = i; + } +} + +int CIVPMaterialManager::RemapIVPMaterialIndex( int ivpMaterialIndex ) const +{ + if ( ivpMaterialIndex > 127 ) + return ivpMaterialIndex; + + return m_propMap[ivpMaterialIndex]; +} + +// remap the incoming (from IVP) index and get the appropriate material +// note that ivp will only supply indices between 1 and 127 +IVP_Material *CIVPMaterialManager::get_material_by_index(IVP_Real_Object *pObject, const IVP_U_Point *world_position, int index) +{ + IVP_Material *tmp = m_props->GetIVPMaterial( RemapIVPMaterialIndex(index) ); + Assert(tmp); + if ( tmp ) + { + return tmp; + } + else + { + return m_props->GetIVPMaterial( m_props->GetSurfaceIndex( "default" ) ); + } +} + +// Installs a LUT for remapping IVP material indices to physprop indices +// A table of the names of the materials in index order is stored with the +// compiled bsp file. This is then remapped dynamically without touching the +// per-triangle indices on load. If we wanted to support multiple LUTs, it would +// be better to preprocess/remap the triangles in the collision models at load time +void CIVPMaterialManager::SetPropMap( int *map, int mapSize ) +{ + // ??? just ignore any extra bits + if ( mapSize > 128 ) + { + mapSize = 128; + } + + for ( int i = 0; i < mapSize; i++ ) + { + m_propMap[i] = (unsigned short)map[i]; + } +} diff --git a/vphysics/physics_material.h b/vphysics/physics_material.h new file mode 100644 index 0000000..679d638 --- /dev/null +++ b/vphysics/physics_material.h @@ -0,0 +1,34 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_MATERIAL_H +#define PHYSICS_MATERIAL_H +#pragma once + +#include "vphysics_interface.h" +class IVP_Material; +class IVP_Material_Manager; + +class IPhysicsSurfacePropsInternal : public IPhysicsSurfaceProps +{ +public: + virtual IVP_Material *GetIVPMaterial( int materialIndex ) = 0; + + virtual int GetIVPMaterialIndex( const IVP_Material *pIVP ) const = 0; + virtual IVP_Material_Manager *GetIVPManager( void ) = 0; + virtual int RemapIVPMaterialIndex( int ivpMaterialIndex ) const = 0; +}; + +extern IPhysicsSurfacePropsInternal *physprops; + +// Special material indices outside of the normal system +enum +{ + MATERIAL_INDEX_SHADOW = 0xF000, +}; + +#endif // PHYSICS_MATERIAL_H diff --git a/vphysics/physics_motioncontroller.cpp b/vphysics/physics_motioncontroller.cpp new file mode 100644 index 0000000..75d62da --- /dev/null +++ b/vphysics/physics_motioncontroller.cpp @@ -0,0 +1,333 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// + +#ifdef _WIN32 +#pragma warning (disable:4127) +#pragma warning (disable:4244) +#endif + +#include "cbase.h" +#include "ivp_controller.hxx" + +#include "physics_motioncontroller.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +struct vphysics_save_motioncontroller_t +{ + CUtlVector<IPhysicsObject *> m_objectList; + int m_nPriority; + + DECLARE_SIMPLE_DATADESC(); +}; + + +BEGIN_SIMPLE_DATADESC( vphysics_save_motioncontroller_t ) + DEFINE_VPHYSPTR_UTLVECTOR( m_objectList ), + DEFINE_FIELD( m_nPriority, FIELD_INTEGER ), +END_DATADESC() + + + +class CPhysicsMotionController : public IVP_Controller_Independent, public IPhysicsMotionController +{ +public: + CPhysicsMotionController( IMotionEvent *pHandler, CPhysicsEnvironment *pVEnv ); + virtual ~CPhysicsMotionController( void ); + virtual void do_simulation_controller(IVP_Event_Sim *event,IVP_U_Vector<IVP_Core> *core_list); + virtual IVP_CONTROLLER_PRIORITY get_controller_priority(); + virtual void core_is_going_to_be_deleted_event(IVP_Core *core) + { + m_coreList.FindAndRemove( core ); + } + virtual const char *get_controller_name() { return "vphysics:motion"; } + + virtual void SetEventHandler( IMotionEvent *handler ); + virtual void AttachObject( IPhysicsObject *pObject, bool checkIfAlreadyAttached ); + virtual void DetachObject( IPhysicsObject *pObject ); + + void RemoveCore( IVP_Core *pCore ); + + // Save/load + void WriteToTemplate( vphysics_save_motioncontroller_t &controllerTemplate ); + void InitFromTemplate( const vphysics_save_motioncontroller_t &controllerTemplate ); + + // returns the number of objects currently attached to the controller + virtual int CountObjects( void ) + { + return m_coreList.Count(); + } + // NOTE: pObjectList is an array with at least CountObjects() allocated + virtual void GetObjects( IPhysicsObject **pObjectList ) + { + for ( int i = 0; i < m_coreList.Count(); i++ ) + { + IVP_Core *pCore = m_coreList[i]; + + IVP_Real_Object *pivp = pCore->objects.element_at(0); + IPhysicsObject *pPhys = static_cast<IPhysicsObject *>(pivp->client_data); + // copy out + pObjectList[i] = pPhys; + } + } + + // detaches all attached objects + virtual void ClearObjects( void ) + { + while ( m_coreList.Count() ) + { + int x = m_coreList.Count()-1; + IVP_Core *pCore = m_coreList[x]; + RemoveCore( pCore ); + } + } + + // wakes up all attached objects + virtual void WakeObjects( void ) + { + for ( int i = 0; i < m_coreList.Count(); i++ ) + { + IVP_Core *pCore = m_coreList[i]; + pCore->ensure_core_to_be_in_simulation(); + } + } + virtual void SetPriority( priority_t priority ); + +private: + IMotionEvent *m_handler; + CUtlVector<IVP_Core *> m_coreList; + CPhysicsEnvironment *m_pVEnv; + int m_priority; +}; + + +CPhysicsMotionController::CPhysicsMotionController( IMotionEvent *pHandler, CPhysicsEnvironment *pVEnv ) +{ + m_handler = pHandler; + m_pVEnv = pVEnv; + SetPriority( MEDIUM_PRIORITY ); +} + +CPhysicsMotionController::~CPhysicsMotionController( void ) +{ + Assert( !m_pVEnv->IsInSimulation() ); + for ( int i = 0; i < m_coreList.Count(); i++ ) + { + m_coreList[i]->rem_core_controller( (IVP_Controller *)this ); + } +} + +void CPhysicsMotionController::do_simulation_controller(IVP_Event_Sim *event,IVP_U_Vector<IVP_Core> *core_list) +{ + if ( m_handler ) + { + for ( int i = 0; i < core_list->len(); i++ ) + { + IVP_U_Float_Point ivpSpeed, ivpRot; + IVP_Core *pCore = core_list->element_at(i); + + IVP_Real_Object *pivp = pCore->objects.element_at(0); + IPhysicsObject *pPhys = static_cast<IPhysicsObject *>(pivp->client_data); + if ( !pPhys->IsMoveable() ) + continue; + + Vector speed; + AngularImpulse rot; + speed.Init(); + rot.Init(); + + IMotionEvent::simresult_e ret = m_handler->Simulate( this, pPhys, event->delta_time, speed, rot ); + + switch( ret ) + { + case IMotionEvent::SIM_NOTHING: + break; + case IMotionEvent::SIM_LOCAL_ACCELERATION: + { + ConvertForceImpulseToIVP( speed, ivpSpeed ); + ConvertAngularImpulseToIVP( rot, ivpRot ); + const IVP_U_Matrix *m_world_f_core = pCore->get_m_world_f_core_PSI(); + // transform to world space + m_world_f_core->inline_vmult3( &ivpSpeed, &ivpSpeed ); + // UNDONE: Put these values into speed change / rot_speed_change instead? + pCore->speed.add_multiple( &ivpSpeed, event->delta_time ); + pCore->rot_speed.add_multiple( &ivpRot, event->delta_time ); + } + break; + case IMotionEvent::SIM_LOCAL_FORCE: + { + ConvertForceImpulseToIVP( speed, ivpSpeed ); + ConvertAngularImpulseToIVP( rot, ivpRot ); + const IVP_U_Matrix *m_world_f_core = pCore->get_m_world_f_core_PSI(); + // transform to world space + m_world_f_core->inline_vmult3( &ivpSpeed, &ivpSpeed ); + pCore->center_push_core_multiple_ws( &ivpSpeed, event->delta_time ); + pCore->rot_push_core_multiple_cs( &ivpRot, event->delta_time ); + } + break; + case IMotionEvent::SIM_GLOBAL_ACCELERATION: + { + ConvertAngularImpulseToIVP( rot, ivpRot ); + ConvertForceImpulseToIVP( speed, ivpSpeed ); + pCore->speed.add_multiple( &ivpSpeed, event->delta_time ); + pCore->rot_speed.add_multiple( &ivpRot, event->delta_time ); + } + break; + case IMotionEvent::SIM_GLOBAL_FORCE: + { + ConvertAngularImpulseToIVP( rot, ivpRot ); + ConvertForceImpulseToIVP( speed, ivpSpeed ); + pCore->center_push_core_multiple_ws( &ivpSpeed, event->delta_time ); + pCore->rot_push_core_multiple_cs( &ivpRot, event->delta_time ); + } + break; + } + pCore->apply_velocity_limit(); + } + } +} + + +IVP_CONTROLLER_PRIORITY CPhysicsMotionController::get_controller_priority() +{ + return (IVP_CONTROLLER_PRIORITY) m_priority; +} + +void CPhysicsMotionController::SetPriority( priority_t priority ) +{ + switch ( priority ) + { + case LOW_PRIORITY: + m_priority = IVP_CP_CONSTRAINTS_MIN; + break; + default: + case MEDIUM_PRIORITY: + m_priority = IVP_CP_MOTION; + break; + case HIGH_PRIORITY: + m_priority = IVP_CP_FORCEFIELDS+1; + break; + } +} + +void CPhysicsMotionController::SetEventHandler( IMotionEvent *handler ) +{ + m_handler = handler; +} + +void CPhysicsMotionController::AttachObject( IPhysicsObject *pObject, bool checkIfAlreadyAttached ) +{ + // BUGBUG: Sometimes restore comes back with a NULL, REVISIT + if ( !pObject || pObject->IsStatic() ) + return; + + CPhysicsObject *pPhys = static_cast<CPhysicsObject *>(pObject); + IVP_Real_Object *pIVP = pPhys->GetObject(); + IVP_Core *pCore = pIVP->get_core(); + + // UNDONE: On save/load, trigger-based motion controllers re-attach their objects. + // UNDONE: Do something cheaper about this? + // OPTIMIZE: Linear search here? + if ( checkIfAlreadyAttached ) + { + int index = m_coreList.Find(pCore); + if ( m_coreList.IsValidIndex(index) ) + { + DevMsg(1,"Attached core twice!!!\n"); + return; + } + } + + m_coreList.AddToTail( pCore ); + + MEM_ALLOC_CREDIT(); + pCore->add_core_controller( (IVP_Controller *)this ); +} + + +void CPhysicsMotionController::RemoveCore( IVP_Core *pCore ) +{ + int index = m_coreList.Find(pCore); + if ( !m_coreList.IsValidIndex(index) ) + { +#if DEBUG + Msg("removed invalid core !!!\n"); +#endif + return; + } + //Assert( !m_pVEnv->IsInSimulation() ); + m_coreList.Remove( index ); + pCore->rem_core_controller( static_cast<IVP_Controller_Independent *>(this) ); +} + + +void CPhysicsMotionController::DetachObject( IPhysicsObject *pObject ) +{ + CPhysicsObject *pPhys = static_cast<CPhysicsObject *>(pObject); + IVP_Real_Object *pIVP = pPhys->GetObject(); + IVP_Core *core = pIVP->get_core(); + + RemoveCore(core); +} + +// Save/load +void CPhysicsMotionController::WriteToTemplate( vphysics_save_motioncontroller_t &controllerTemplate ) +{ + controllerTemplate.m_nPriority = m_priority; + + int nObjectCount = CountObjects(); + controllerTemplate.m_objectList.AddMultipleToTail( nObjectCount ); + GetObjects( controllerTemplate.m_objectList.Base() ); +} + +void CPhysicsMotionController::InitFromTemplate( const vphysics_save_motioncontroller_t &controllerTemplate ) +{ + m_priority = controllerTemplate.m_nPriority; + + int nObjectCount = controllerTemplate.m_objectList.Count(); + for ( int i = 0; i < nObjectCount; ++i ) + { + AttachObject( controllerTemplate.m_objectList[i], true ); + } +} + + +IPhysicsMotionController *CreateMotionController( CPhysicsEnvironment *pPhysEnv, IMotionEvent *pHandler ) +{ + if ( !pHandler ) + return NULL; + + return new CPhysicsMotionController( pHandler, pPhysEnv ); +} + +bool SavePhysicsMotionController( const physsaveparams_t ¶ms, IPhysicsMotionController *pMotionController ) +{ + vphysics_save_motioncontroller_t controllerTemplate; + memset( &controllerTemplate, 0, sizeof(controllerTemplate) ); + + CPhysicsMotionController *pControllerImp = static_cast<CPhysicsMotionController*>(pMotionController); + pControllerImp->WriteToTemplate( controllerTemplate ); + params.pSave->WriteAll( &controllerTemplate ); + + return true; +} + +bool RestorePhysicsMotionController( const physrestoreparams_t ¶ms, IPhysicsMotionController **ppMotionController ) +{ + CPhysicsMotionController *pControllerImp = new CPhysicsMotionController( NULL, static_cast<CPhysicsEnvironment *>(params.pEnvironment) ); + + vphysics_save_motioncontroller_t controllerTemplate; + memset( &controllerTemplate, 0, sizeof(controllerTemplate) ); + params.pRestore->ReadAll( &controllerTemplate ); + + pControllerImp->InitFromTemplate( controllerTemplate ); + *ppMotionController = pControllerImp; + + return true; +} diff --git a/vphysics/physics_motioncontroller.h b/vphysics/physics_motioncontroller.h new file mode 100644 index 0000000..a549807 --- /dev/null +++ b/vphysics/physics_motioncontroller.h @@ -0,0 +1,20 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_MOTIONCONTROLLER_H +#define PHYSICS_MOTIONCONTROLLER_H +#ifdef _WIN32 +#pragma once +#endif + +class IPhysicsMotionController; +class CPhysicsEnvironment; +class IMotionEvent; + +IPhysicsMotionController *CreateMotionController( CPhysicsEnvironment *pEnv, IMotionEvent *pHandler ); + +#endif // PHYSICS_MOTIONCONTROLLER_H diff --git a/vphysics/physics_object.cpp b/vphysics/physics_object.cpp new file mode 100644 index 0000000..601772d --- /dev/null +++ b/vphysics/physics_object.cpp @@ -0,0 +1,2001 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// +#include "cbase.h" +#include "ivp_surman_polygon.hxx" +#include "ivp_compact_ledge.hxx" +#include "ivp_compact_ledge_solver.hxx" +#include "ivp_mindist.hxx" +#include "ivp_friction.hxx" +#include "ivp_phantom.hxx" +#include "ivp_listener_collision.hxx" +#include "ivp_clustering_visualizer.hxx" +#include "ivp_anomaly_manager.hxx" +#include "ivp_collision_filter.hxx" + +#include "hk_mopp/ivp_surman_mopp.hxx" +#include "hk_mopp/ivp_compact_mopp.hxx" + +#include "ivp_compact_surface.hxx" +#include "physics_trace.h" +#include "physics_shadow.h" +#include "physics_friction.h" +#include "physics_constraint.h" +#include "bspflags.h" +#include "vphysics/player_controller.h" +#include "vphysics/friction.h" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +extern IPhysicsCollision *physcollision; + +// UNDONE: Make this a stack variable / member variable of some save/load object or function? +// NOTE: This keeps a list of objects who were saved while asleep, but not created asleep +// So some info will be lost unless it's regenerated after loading. +struct postrestore_objectlist_t +{ + CPhysicsObject *pObject; + bool growFriction; + bool enableCollisions; + + void Defaults() + { + pObject = NULL; + growFriction = false; + enableCollisions = false; + } +}; + +static CUtlVector<postrestore_objectlist_t> g_PostRestoreObjectList; + +// This angular basis is the integral of each differential drag area's torque over the whole OBB +// For each axis, each face, the integral is (1/Iaxis) * (1/3 * w^2l^3 + 1/2 * w^4l + lw^2h^2) +// l,w, & h are half widths - where l is in the direction of the axis, w is in the plane (l/w plane) of the face, +// and h is perpendicular to the face. So for each axis, you sum up this integral over 2 pairs of faces +// (this function returns the integral for one pair of opposite faces, not one face) +static float AngDragIntegral( float invInertia, float l, float w, float h ) +{ + float w2 = w*w; + float l2 = l*l; + float h2 = h*h; + + return invInertia * ( (1.f/3.f)*w2*l*l2 + 0.5 * w2*w2*l + l*w2*h2 ); +} + + +CPhysicsObject::CPhysicsObject( void ) +{ +#ifdef _WIN32 + void *pData = ((char *)this) + sizeof(void *); // offset beyond vtable + int dataSize = sizeof(*this) - sizeof(void *); + + Assert( pData == &m_pGameData ); + + memset( pData, 0, dataSize ); +#elif POSIX + + //!!HACK HACK - rework this if we ever change compiler versions (from gcc 3.2!!!) + void *pData = ((char *)this) + sizeof(void *); // offset beyond vtable + int dataSize = sizeof(*this) - sizeof(void *); + + Assert( pData == &m_pGameData ); + + memset( pData, 0, dataSize ); +#else +#error +#endif + + // HACKHACK: init this as a sphere until someone attaches a surfacemanager + m_collideType = COLLIDE_BALL; + m_contentsMask = CONTENTS_SOLID; + m_hasTouchedDynamic = 0; +} + +void CPhysicsObject::Init( const CPhysCollide *pCollisionModel, IVP_Real_Object *pObject, int materialIndex, float volume, float drag, float angDrag ) +{ + m_pCollide = pCollisionModel; + m_materialIndex = materialIndex; + m_pObject = pObject; + pObject->client_data = (void *)this; + m_pGameData = NULL; + m_gameFlags = 0; + m_gameIndex = 0; + m_sleepState = OBJ_SLEEP; // objects start asleep + m_callbacks = CALLBACK_GLOBAL_COLLISION|CALLBACK_GLOBAL_FRICTION|CALLBACK_FLUID_TOUCH|CALLBACK_GLOBAL_TOUCH|CALLBACK_GLOBAL_COLLIDE_STATIC|CALLBACK_DO_FLUID_SIMULATION; + m_activeIndex = 0xFFFF; + m_pShadow = NULL; + m_shadowTempGravityDisable = false; + m_forceSilentDelete = false; + m_dragBasis = vec3_origin; + m_angDragBasis = vec3_origin; + + if ( !IsStatic() && GetCollide() ) + { + RecomputeDragBases(); + } + else + { + drag = 0; + angDrag = 0; + } + + m_dragCoefficient = drag; + m_angDragCoefficient = angDrag; + + SetVolume( volume ); +} + +CPhysicsObject::~CPhysicsObject( void ) +{ + RemoveShadowController(); + + if ( m_pObject ) + { + // prevents callbacks to the game code / unlink from this object + m_callbacks = 0; + m_pGameData = 0; + m_pObject->client_data = 0; + + IVP_Core *pCore = m_pObject->get_core(); + if ( pCore->physical_unmoveable == IVP_TRUE && pCore->controllers_of_core.n_elems ) + { + // go ahead and notify them if this happens in the real world + for(int i = pCore->controllers_of_core.len()-1; i >=0 ;i-- ) + { + IVP_Controller *my_controller = pCore->controllers_of_core.element_at(i); + my_controller->core_is_going_to_be_deleted_event(pCore); + Assert(my_controller==pCore->environment->get_gravity_controller()); + } + } + + // UNDONE: Don't free the surface manager here + // UNDONE: Remove reference to it by calling something in physics_collide + IVP_SurfaceManager *pSurman = GetSurfaceManager(); + CPhysicsEnvironment *pVEnv = GetVPhysicsEnvironment(); + + // BUGBUG: Sometimes IVP will call a "revive" on the object we're deleting! + MEM_ALLOC_CREDIT(); + if ( m_forceSilentDelete || (pVEnv && pVEnv->ShouldQuickDelete()) || !m_hasTouchedDynamic ) + { + m_pObject->delete_silently(); + } + else + { + m_pObject->delete_and_check_vicinity(); + } + delete pSurman; + } +} + +void CPhysicsObject::Wake( void ) +{ + m_pObject->ensure_in_simulation(); +} + +// supported +void CPhysicsObject::Sleep( void ) +{ + m_pObject->disable_simulation(); +} + + +bool CPhysicsObject::IsAsleep() const +{ + if ( m_sleepState == OBJ_AWAKE ) + return false; + + // double-check that we aren't pending + if ( m_pObject->get_core()->is_in_wakeup_vec ) + return false; + + return true; +} + +void CPhysicsObject::NotifySleep( void ) +{ + if ( m_sleepState == OBJ_AWAKE ) + { + m_sleepState = OBJ_STARTSLEEP; + } + else + { + // UNDONE: This fails sometimes and we get sleep calls for a sleeping object, debug? + //Assert(m_sleepState==OBJ_STARTSLEEP); + m_sleepState = OBJ_SLEEP; + } +} + + +void CPhysicsObject::NotifyWake( void ) +{ + m_asleepSinceCreation = false; + m_sleepState = OBJ_AWAKE; +} + + +void CPhysicsObject::SetCallbackFlags( unsigned short callbackflags ) +{ +#if IVP_ENABLE_VISUALIZER + unsigned short changedFlags = m_callbacks ^ callbackflags; + if ( changedFlags & CALLBACK_MARKED_FOR_TEST ) + { + if ( callbackflags & CALLBACK_MARKED_FOR_TEST ) + { + ENABLE_SHORTRANGE_VISUALIZATION(m_pObject); + ENABLE_LONGRANGE_VISUALIZATION(m_pObject); + } + else + { + DISABLE_SHORTRANGE_VISUALIZATION(m_pObject); + DISABLE_LONGRANGE_VISUALIZATION(m_pObject); + } + } +#endif + m_callbacks = callbackflags; + +} + + +unsigned short CPhysicsObject::GetCallbackFlags() const +{ + return m_callbacks; +} + + +void CPhysicsObject::SetGameFlags( unsigned short userFlags ) +{ + m_gameFlags = userFlags; +} + +unsigned short CPhysicsObject::GetGameFlags() const +{ + return m_gameFlags; +} + + +void CPhysicsObject::SetGameIndex( unsigned short gameIndex ) +{ + m_gameIndex = gameIndex; +} + +unsigned short CPhysicsObject::GetGameIndex() const +{ + return m_gameIndex; +} + +bool CPhysicsObject::IsStatic() const +{ + if ( m_pObject->get_core()->physical_unmoveable ) + return true; + + return false; +} + + +void CPhysicsObject::EnableCollisions( bool enable ) +{ + if ( enable ) + { + m_callbacks |= CALLBACK_ENABLING_COLLISION; + BEGIN_IVP_ALLOCATION(); + m_pObject->enable_collision_detection( IVP_TRUE ); + END_IVP_ALLOCATION(); + m_callbacks &= ~CALLBACK_ENABLING_COLLISION; + } + else + { + if ( IsCollisionEnabled() ) + { + // Delete all contact points with this physics object because it's collision is becoming disabled + IPhysicsFrictionSnapshot *pSnapshot = CreateFrictionSnapshot(); + while ( pSnapshot->IsValid() ) + { + pSnapshot->MarkContactForDelete(); + pSnapshot->NextFrictionData(); + } + pSnapshot->DeleteAllMarkedContacts( true ); + DestroyFrictionSnapshot( pSnapshot ); + } + + m_pObject->enable_collision_detection( IVP_FALSE ); + } +} + +void CPhysicsObject::RecheckCollisionFilter() +{ + if ( CallbackFlags() & CALLBACK_MARKED_FOR_DELETE ) + return; + + m_callbacks |= CALLBACK_ENABLING_COLLISION; + BEGIN_IVP_ALLOCATION(); + m_pObject->recheck_collision_filter(); + // UNDONE: do a RecheckContactPoints() here? + END_IVP_ALLOCATION(); + m_callbacks &= ~CALLBACK_ENABLING_COLLISION; +} + +void CPhysicsObject::RecheckContactPoints() +{ + IVP_Environment *pEnv = m_pObject->get_environment(); + IVP_Collision_Filter *coll_filter = pEnv->get_collision_filter(); + IPhysicsFrictionSnapshot *pSnapshot = CreateFrictionSnapshot(); + while ( pSnapshot->IsValid() ) + { + CPhysicsObject *pOther = static_cast<CPhysicsObject *>(pSnapshot->GetObject(1)); + if ( !coll_filter->check_objects_for_collision_detection( m_pObject, pOther->m_pObject ) ) + { + pSnapshot->MarkContactForDelete(); + } + pSnapshot->NextFrictionData(); + } + pSnapshot->DeleteAllMarkedContacts( true ); + DestroyFrictionSnapshot( pSnapshot ); +} + +CPhysicsEnvironment *CPhysicsObject::GetVPhysicsEnvironment() +{ + return (CPhysicsEnvironment *) (m_pObject->get_environment()->client_data); +} + +const CPhysicsEnvironment *CPhysicsObject::GetVPhysicsEnvironment() const +{ + return (CPhysicsEnvironment *) (m_pObject->get_environment()->client_data); +} + + +bool CPhysicsObject::IsControlling( const IVP_Controller *pController ) const +{ + IVP_Core *pCore = m_pObject->get_core(); + for ( int i = 0; i < pCore->controllers_of_core.len(); i++ ) + { + // already controlling this core? + if ( pCore->controllers_of_core.element_at(i) == pController ) + return true; + } + + return false; +} + +bool CPhysicsObject::IsGravityEnabled() const +{ + if ( !IsStatic() ) + { + return IsControlling( m_pObject->get_core()->environment->get_gravity_controller() ); + } + + return false; +} + +bool CPhysicsObject::IsDragEnabled() const +{ + if ( !IsStatic() ) + { + return IsControlling( GetVPhysicsEnvironment()->GetDragController() ); + } + + return false; +} + + +bool CPhysicsObject::IsMotionEnabled() const +{ + return m_pObject->get_core()->pinned ? false : true; +} + + +bool CPhysicsObject::IsMoveable() const +{ + if ( IsStatic() || !IsMotionEnabled() ) + return false; + return true; +} + + +void CPhysicsObject::EnableGravity( bool enable ) +{ + if ( IsStatic() ) + return; + + + bool isEnabled = IsGravityEnabled(); + + if ( enable == isEnabled ) + return; + + IVP_Controller *pGravity = m_pObject->get_core()->environment->get_gravity_controller(); + if ( enable ) + { + m_pObject->get_core()->add_core_controller( pGravity ); + } + else + { + m_pObject->get_core()->rem_core_controller( pGravity ); + } +} + +void CPhysicsObject::EnableDrag( bool enable ) +{ + if ( IsStatic() ) + return; + + bool isEnabled = IsDragEnabled(); + + if ( enable == isEnabled ) + return; + + IVP_Controller *pDrag = GetVPhysicsEnvironment()->GetDragController(); + + if ( enable ) + { + m_pObject->get_core()->add_core_controller( pDrag ); + } + else + { + m_pObject->get_core()->rem_core_controller( pDrag ); + } +} + + +void CPhysicsObject::SetDragCoefficient( float *pDrag, float *pAngularDrag ) +{ + if ( pDrag ) + { + m_dragCoefficient = *pDrag; + } + if ( pAngularDrag ) + { + m_angDragCoefficient = *pAngularDrag; + } + + EnableDrag( m_dragCoefficient || m_angDragCoefficient ); +} + + +void CPhysicsObject::RecomputeDragBases() +{ + if ( IsStatic() || !GetCollide() ) + return; + + // Basically we are computing drag as an OBB. Get OBB extents for projection + // scale those extents by appropriate mass/inertia to compute velocity directly (not force) + // in the controller + // NOTE: Compute these even if drag coefficients are zero, because the drag coefficient could change later + + // Get an AABB for this object and use the area of each side as a basis for approximating cross-section area for drag + Vector dragMins, dragMaxs; + // NOTE: coordinates in/out of physcollision are in HL units, not IVP + // PERFORMANCE: Cache this? Expensive. + physcollision->CollideGetAABB( &dragMins, &dragMaxs, GetCollide(), vec3_origin, vec3_angle ); + + Vector areaFractions = physcollision->CollideGetOrthographicAreas( GetCollide() ); + Vector delta = dragMaxs - dragMins; + ConvertPositionToIVP( delta.x, delta.y, delta.z ); + delta.x = fabsf(delta.x); + delta.y = fabsf(delta.y); + delta.z = fabsf(delta.z); + // dragBasis is now the area of each side + m_dragBasis.x = delta.y * delta.z * areaFractions.x; + m_dragBasis.y = delta.x * delta.z * areaFractions.y; + m_dragBasis.z = delta.x * delta.y * areaFractions.z; + m_dragBasis *= GetInvMass(); + + const IVP_U_Float_Point *pInvRI = m_pObject->get_core()->get_inv_rot_inertia(); + + // This angular basis is the integral of each differential drag area's torque over the whole OBB + // need half lengths for this integral + delta *= 0.5; + // rotation about the x axis + m_angDragBasis.x = areaFractions.z * AngDragIntegral( pInvRI->k[0], delta.x, delta.y, delta.z ) + areaFractions.y * AngDragIntegral( pInvRI->k[0], delta.x, delta.z, delta.y ); + // rotation about the y axis + m_angDragBasis.y = areaFractions.z * AngDragIntegral( pInvRI->k[1], delta.y, delta.x, delta.z ) + areaFractions.x * AngDragIntegral( pInvRI->k[1], delta.y, delta.z, delta.x ); + // rotation about the z axis + m_angDragBasis.z = areaFractions.y * AngDragIntegral( pInvRI->k[2], delta.z, delta.x, delta.y ) + areaFractions.x * AngDragIntegral( pInvRI->k[2], delta.z, delta.y, delta.x ); +} + + + +void CPhysicsObject::EnableMotion( bool enable ) +{ + if ( IsStatic() ) + return; + + bool isMoveable = IsMotionEnabled(); + + // no change + if ( isMoveable == enable ) + return; + + BEGIN_IVP_ALLOCATION(); + m_pObject->set_pinned( enable ? IVP_FALSE : IVP_TRUE ); + END_IVP_ALLOCATION(); + + if ( enable && IsHinged() ) + { + BecomeHinged( m_hingedAxis-1 ); + } + RecheckCollisionFilter(); + RecheckContactPoints(); +} + +bool CPhysicsObject::IsControlledByGame() const +{ + if (m_pShadow && !m_pShadow->IsPhysicallyControlled()) + return true; + + if ( CallbackFlags() & CALLBACK_IS_PLAYER_CONTROLLER ) + return true; + + return false; +} + +IPhysicsFrictionSnapshot *CPhysicsObject::CreateFrictionSnapshot() +{ + return ::CreateFrictionSnapshot( m_pObject ); +} + +void CPhysicsObject::DestroyFrictionSnapshot( IPhysicsFrictionSnapshot *pSnapshot ) +{ + ::DestroyFrictionSnapshot(pSnapshot); +} + +bool CPhysicsObject::IsMassCenterAtDefault() const +{ + // this is the actual mass center of the object as created + Vector massCenterHL = GetMassCenterLocalSpace(); + + // Get the default mass center to see if it has been changed + IVP_U_Float_Point massCenterIVPDefault; + Vector massCenterHLDefault; + GetObject()->get_surface_manager()->get_mass_center( &massCenterIVPDefault ); + ConvertPositionToHL( massCenterIVPDefault, massCenterHLDefault ); + float delta = (massCenterHLDefault - massCenterHL).Length(); + + return ( delta <= g_PhysicsUnits.collisionSweepIncrementalEpsilon ) ? true : false; +} + +Vector CPhysicsObject::GetMassCenterLocalSpace() const +{ + if ( m_pObject->flags.shift_core_f_object_is_zero ) + return vec3_origin; + + Vector out; + ConvertPositionToHL( *m_pObject->get_shift_core_f_object(), out ); + // core shift is what you add to the mass center to get the origin + // so we want the negative core shift (origin relative position of the mass center) + return -out; +} + + +void CPhysicsObject::SetGameData( void *pGameData ) +{ + m_pGameData = pGameData; +} + +void *CPhysicsObject::GetGameData( void ) const +{ + return m_pGameData; +} + +void CPhysicsObject::SetMass( float mass ) +{ + bool reset = false; + + if ( !IsMoveable() ) + { + reset = true; + EnableMotion(true); + } + + Assert( mass > 0 ); + mass = clamp( mass, 0, VPHYSICS_MAX_MASS ); // NOTE: Allow zero procedurally, but not by initialization + m_pObject->change_mass( mass ); + SetVolume( m_volume ); + RecomputeDragBases(); + if ( reset ) + { + EnableMotion(false); + } +} + +float CPhysicsObject::GetMass( void ) const +{ + return m_pObject->get_core()->get_mass(); +} + +float CPhysicsObject::GetInvMass( void ) const +{ + return m_pObject->get_core()->get_inv_mass(); +} + +Vector CPhysicsObject::GetInertia( void ) const +{ + const IVP_U_Float_Point *pRI = m_pObject->get_core()->get_rot_inertia(); + + Vector hlInertia; + ConvertDirectionToHL( *pRI, hlInertia ); + VectorAbs( hlInertia, hlInertia ); + return hlInertia; +} + +Vector CPhysicsObject::GetInvInertia( void ) const +{ + const IVP_U_Float_Point *pRI = m_pObject->get_core()->get_inv_rot_inertia(); + + Vector hlInvInertia; + ConvertDirectionToHL( *pRI, hlInvInertia ); + VectorAbs( hlInvInertia, hlInvInertia ); + return hlInvInertia; +} + + +void CPhysicsObject::SetInertia( const Vector &inertia ) +{ + IVP_U_Float_Point ri; + ConvertDirectionToIVP( inertia, ri ); + ri.k[0] = IVP_Inline_Math::fabsd(ri.k[0]); + ri.k[1] = IVP_Inline_Math::fabsd(ri.k[1]); + ri.k[2] = IVP_Inline_Math::fabsd(ri.k[2]); + m_pObject->get_core()->set_rotation_inertia( &ri ); +} + + +void CPhysicsObject::GetDamping( float *speed, float *rot ) const +{ + IVP_Core *pCore = m_pObject->get_core(); + if ( speed ) + { + *speed = pCore->speed_damp_factor; + } + if ( rot ) + { + *rot = pCore->rot_speed_damp_factor.k[0]; + } +} + +void CPhysicsObject::SetDamping( const float *speed, const float *rot ) +{ + IVP_Core *pCore = m_pObject->get_core(); + if ( speed ) + { + pCore->speed_damp_factor = *speed; + } + if ( rot ) + { + pCore->rot_speed_damp_factor.set( *rot, *rot, *rot ); + } +} + +void CPhysicsObject::SetVolume( float volume ) +{ + m_volume = volume; + if ( volume != 0.f ) + { + // minimum volume is 5 cubic inches - otherwise buoyancy can get unstable + if ( volume < 5.0f ) + { + volume = 5.0f; + } + volume *= HL2IVP_FACTOR*HL2IVP_FACTOR*HL2IVP_FACTOR; + float density = GetMass() / volume; + float matDensity; + physprops->GetPhysicsProperties( GetMaterialIndexInternal(), &matDensity, NULL, NULL, NULL ); + m_buoyancyRatio = density / matDensity; + } + else + { + m_buoyancyRatio = 1.0f; + } +} + +float CPhysicsObject::GetVolume() const +{ + return m_volume; +} + + +void CPhysicsObject::SetBuoyancyRatio( float ratio ) +{ + m_buoyancyRatio = ratio; +} + +void CPhysicsObject::SetContents( unsigned int contents ) +{ + m_contentsMask = contents; +} + +// converts HL local units to HL world units +void CPhysicsObject::LocalToWorld( Vector *worldPosition, const Vector &localPosition ) const +{ + matrix3x4_t matrix; + GetPositionMatrix( &matrix ); + // copy in case the src == dest + VectorTransform( Vector(localPosition), matrix, *worldPosition ); +} + +// Converts world HL units to HL local/object units +void CPhysicsObject::WorldToLocal( Vector *localPosition, const Vector &worldPosition ) const +{ + matrix3x4_t matrix; + GetPositionMatrix( &matrix ); + // copy in case the src == dest + VectorITransform( Vector(worldPosition), matrix, *localPosition ); +} + +void CPhysicsObject::LocalToWorldVector( Vector *worldVector, const Vector &localVector ) const +{ + matrix3x4_t matrix; + GetPositionMatrix( &matrix ); + // copy in case the src == dest + VectorRotate( Vector(localVector), matrix, *worldVector ); +} + +void CPhysicsObject::WorldToLocalVector( Vector *localVector, const Vector &worldVector ) const +{ + matrix3x4_t matrix; + GetPositionMatrix( &matrix ); + // copy in case the src == dest + VectorIRotate( Vector(worldVector), matrix, *localVector ); +} + + +// Apply force impulse (momentum) to the object +void CPhysicsObject::ApplyForceCenter( const Vector &forceVector ) +{ + if ( !IsMoveable() ) + return; + + IVP_U_Float_Point tmp; + + ConvertForceImpulseToIVP( forceVector, tmp ); + IVP_Core *core = m_pObject->get_core(); + tmp.mult( core->get_inv_mass() ); + m_pObject->async_add_speed_object_ws( &tmp ); + ClampVelocity(); +} + +void CPhysicsObject::ApplyForceOffset( const Vector &forceVector, const Vector &worldPosition ) +{ + if ( !IsMoveable() ) + return; + + IVP_U_Point pos; + IVP_U_Float_Point force; + + ConvertForceImpulseToIVP( forceVector, force ); + ConvertPositionToIVP( worldPosition, pos ); + + IVP_Core *core = m_pObject->get_core(); + core->async_push_core_ws( &pos, &force ); + Wake(); + ClampVelocity(); +} + +void CPhysicsObject::CalculateForceOffset( const Vector &forceVector, const Vector &worldPosition, Vector *centerForce, AngularImpulse *centerTorque ) const +{ + IVP_U_Point pos; + IVP_U_Float_Point force; + + ConvertPositionToIVP( forceVector, force ); + ConvertPositionToIVP( worldPosition, pos ); + + IVP_Core *core = m_pObject->get_core(); + + const IVP_U_Matrix *m_world_f_core = core->get_m_world_f_core_PSI(); + + IVP_U_Float_Point point_d_ws; + point_d_ws.subtract(&pos, m_world_f_core->get_position()); + + IVP_U_Float_Point cross_point_dir; + + cross_point_dir.calc_cross_product( &point_d_ws, &force); + m_world_f_core->inline_vimult3( &cross_point_dir, &cross_point_dir); + + ConvertAngularImpulseToHL( cross_point_dir, *centerTorque ); + ConvertForceImpulseToHL( force, *centerForce ); +} + +void CPhysicsObject::CalculateVelocityOffset( const Vector &forceVector, const Vector &worldPosition, Vector *centerVelocity, AngularImpulse *centerAngularVelocity ) const +{ + IVP_U_Point pos; + IVP_U_Float_Point force; + + ConvertForceImpulseToIVP( forceVector, force ); + ConvertPositionToIVP( worldPosition, pos ); + + IVP_Core *core = m_pObject->get_core(); + + const IVP_U_Matrix *m_world_f_core = core->get_m_world_f_core_PSI(); + + IVP_U_Float_Point point_d_ws; + point_d_ws.subtract(&pos, m_world_f_core->get_position()); + + IVP_U_Float_Point cross_point_dir; + + cross_point_dir.calc_cross_product( &point_d_ws, &force); + m_world_f_core->inline_vimult3( &cross_point_dir, &cross_point_dir); + + cross_point_dir.set_pairwise_mult( &cross_point_dir, core->get_inv_rot_inertia()); + ConvertAngularImpulseToHL( cross_point_dir, *centerAngularVelocity ); + force.set_multiple( &force, core->get_inv_mass() ); + ConvertForceImpulseToHL( force, *centerVelocity ); +} + +void CPhysicsObject::ApplyTorqueCenter( const AngularImpulse &torqueImpulse ) +{ + if ( !IsMoveable() ) + return; + IVP_U_Float_Point ivpTorque; + ConvertAngularImpulseToIVP( torqueImpulse, ivpTorque ); + IVP_Core *core = m_pObject->get_core(); + core->async_rot_push_core_multiple_ws( &ivpTorque, 1.0 ); + Wake(); + ClampVelocity(); +} + +void CPhysicsObject::GetPosition( Vector *worldPosition, QAngle *angles ) const +{ + IVP_U_Matrix matrix; + m_pObject->get_m_world_f_object_AT( &matrix ); + if ( worldPosition ) + { + ConvertPositionToHL( matrix.vv, *worldPosition ); + } + + if ( angles ) + { + ConvertRotationToHL( matrix, *angles ); + } +} + + +void CPhysicsObject::GetPositionMatrix( matrix3x4_t *positionMatrix ) const +{ + IVP_U_Matrix matrix; + m_pObject->get_m_world_f_object_AT( &matrix ); + ConvertMatrixToHL( matrix, *positionMatrix ); +} + + +void CPhysicsObject::GetImplicitVelocity( Vector *velocity, AngularImpulse *angularVelocity ) const +{ + if ( !velocity && !angularVelocity ) + return; + + IVP_Core *core = m_pObject->get_core(); + if ( velocity ) + { + // just convert the cached dx + ConvertPositionToHL( core->delta_world_f_core_psis, *velocity ); + } + + if ( angularVelocity ) + { + // compute the relative transform that was actually integrated in the last psi + IVP_U_Quat q_core_f_core; + q_core_f_core.set_invert_mult( &core->q_world_f_core_last_psi, &core->q_world_f_core_next_psi); + + // now convert that to an axis/angle pair + Quaternion q( q_core_f_core.x, q_core_f_core.y, q_core_f_core.z, q_core_f_core.w ); + AngularImpulse axis; + float angle; + QuaternionAxisAngle( q, axis, angle ); + + // scale it by the timestep to get a velocity + angle *= core->i_delta_time; + + // ConvertDirectionToHL() - convert this ipion direction (in HL type) to HL coords + float tmpY = axis.z; + angularVelocity->z = -axis.y; + angularVelocity->y = tmpY; + angularVelocity->x = axis.x; + + // now scale the axis by the angle to return the data in the correct format + (*angularVelocity) *= angle; + } +} + +void CPhysicsObject::GetVelocity( Vector *velocity, AngularImpulse *angularVelocity ) const +{ + if ( !velocity && !angularVelocity ) + return; + + IVP_Core *core = m_pObject->get_core(); + if ( velocity ) + { + IVP_U_Float_Point speed; + speed.add( &core->speed, &core->speed_change ); + ConvertPositionToHL( speed, *velocity ); + } + + if ( angularVelocity ) + { + IVP_U_Float_Point rotSpeed; + rotSpeed.add( &core->rot_speed, &core->rot_speed_change ); + // xform to HL space + ConvertAngularImpulseToHL( rotSpeed, *angularVelocity ); + } +} + +void CPhysicsObject::GetVelocityAtPoint( const Vector &worldPosition, Vector *pVelocity ) const +{ + IVP_Core *core = m_pObject->get_core(); + IVP_U_Point pos; + ConvertPositionToIVP( worldPosition, pos ); + + IVP_U_Float_Point rotSpeed; + rotSpeed.add( &core->rot_speed, &core->rot_speed_change ); + + IVP_U_Float_Point av_ws; + core->get_m_world_f_core_PSI()->vmult3( &rotSpeed, &av_ws); + + IVP_U_Float_Point pos_rel; + pos_rel.subtract( &pos, core->get_position_PSI()); + IVP_U_Float_Point cross; + cross.inline_calc_cross_product(&av_ws,&pos_rel); + + IVP_U_Float_Point speed; + speed.add(&core->speed, &cross); + speed.add(&core->speed_change); + + ConvertPositionToHL( speed, *pVelocity ); +} + + +// UNDONE: Limit these? +void CPhysicsObject::AddVelocity( const Vector *velocity, const AngularImpulse *angularVelocity ) +{ + Assert(IsMoveable()); + if ( !IsMoveable() ) + return; + IVP_Core *core = m_pObject->get_core(); + + Wake(); + + if ( velocity ) + { + IVP_U_Float_Point ivpVelocity; + ConvertPositionToIVP( *velocity, ivpVelocity ); + core->speed_change.add( &ivpVelocity ); + } + + if ( angularVelocity ) + { + IVP_U_Float_Point ivpAngularVelocity; + ConvertAngularImpulseToIVP( *angularVelocity, ivpAngularVelocity ); + + core->rot_speed_change.add(&ivpAngularVelocity); + } + ClampVelocity(); +} + +void CPhysicsObject::SetPosition( const Vector &worldPosition, const QAngle &angles, bool isTeleport ) +{ + IVP_U_Quat rot; + IVP_U_Point pos; + + if ( m_pShadow ) + { + UpdateShadow( worldPosition, angles, false, 0 ); + } + ConvertPositionToIVP( worldPosition, pos ); + + ConvertRotationToIVP( angles, rot ); + + if ( m_pObject->is_collision_detection_enabled() && isTeleport ) + { + EnableCollisions( false ); + m_pObject->beam_object_to_new_position( &rot, &pos, IVP_FALSE ); + EnableCollisions( true ); + } + else + { + m_pObject->beam_object_to_new_position( &rot, &pos, IVP_FALSE ); + } +} + +void CPhysicsObject::SetPositionMatrix( const matrix3x4_t& matrix, bool isTeleport ) +{ + if ( m_pShadow ) + { + Vector worldPosition; + QAngle angles; + MatrixAngles( matrix, angles ); + MatrixGetColumn( matrix, 3, worldPosition ); + UpdateShadow( worldPosition, angles, false, 0 ); + } + + IVP_U_Quat rot; + IVP_U_Matrix mat; + + ConvertMatrixToIVP( matrix, mat ); + + rot.set_quaternion( &mat ); + + if ( m_pObject->is_collision_detection_enabled() && isTeleport ) + { + EnableCollisions( false ); + m_pObject->beam_object_to_new_position( &rot, &mat.vv, IVP_FALSE ); + EnableCollisions( true ); + } + else + { + m_pObject->beam_object_to_new_position( &rot, &mat.vv, IVP_FALSE ); + } +} + +void CPhysicsObject::SetVelocityInstantaneous( const Vector *velocity, const AngularImpulse *angularVelocity ) +{ + Assert(IsMoveable()); + if ( !IsMoveable() ) + return; + IVP_Core *core = m_pObject->get_core(); + + Wake(); + + if ( velocity ) + { + ConvertPositionToIVP( *velocity, core->speed ); + core->speed_change.set_to_zero(); + } + + if ( angularVelocity ) + { + ConvertAngularImpulseToIVP( *angularVelocity, core->rot_speed ); + core->rot_speed_change.set_to_zero(); + } + ClampVelocity(); +} + +void CPhysicsObject::SetVelocity( const Vector *velocity, const AngularImpulse *angularVelocity ) +{ + if ( !IsMoveable() ) + return; + IVP_Core *core = m_pObject->get_core(); + + Wake(); + + if ( velocity ) + { + ConvertPositionToIVP( *velocity, core->speed_change ); + core->speed.set_to_zero(); + } + + if ( angularVelocity ) + { + ConvertAngularImpulseToIVP( *angularVelocity, core->rot_speed_change ); + core->rot_speed.set_to_zero(); + } + ClampVelocity(); +} + + +void CPhysicsObject::ClampVelocity() +{ + if ( m_pShadow ) + return; + + m_pObject->get_core()->apply_velocity_limit(); +} + +void GetWorldCoordFromSynapse( IVP_Synapse_Friction *pfriction, IVP_U_Point &world ) +{ + world.set(pfriction->get_contact_point()->get_contact_point_ws()); +} + + +bool CPhysicsObject::GetContactPoint( Vector *contactPoint, IPhysicsObject **contactObject ) const +{ + IVP_Synapse_Friction *pfriction = m_pObject->get_first_friction_synapse(); + if ( !pfriction ) + return false; + + if ( contactPoint ) + { + IVP_U_Point world; + GetWorldCoordFromSynapse( pfriction, world ); + ConvertPositionToHL( world, *contactPoint ); + } + if ( contactObject ) + { + IVP_Real_Object *pivp = GetOppositeSynapseObject( pfriction ); + *contactObject = static_cast<IPhysicsObject *>(pivp->client_data); + } + return true; +} + +void CPhysicsObject::SetShadow( float maxSpeed, float maxAngularSpeed, bool allowPhysicsMovement, bool allowPhysicsRotation ) +{ + if ( m_pShadow ) + { + m_pShadow->MaxSpeed( maxSpeed, maxAngularSpeed ); + } + else + { + m_shadowTempGravityDisable = false; + + CPhysicsEnvironment *pVEnv = GetVPhysicsEnvironment(); + m_pShadow = pVEnv->CreateShadowController( this, allowPhysicsMovement, allowPhysicsRotation ); + m_pShadow->MaxSpeed( maxSpeed, maxAngularSpeed ); + // This really should be in the game code, but do this here because the game may (does) use + // shadow/AI control as a collision filter indicator. + RecheckCollisionFilter(); + } +} + +void CPhysicsObject::UpdateShadow( const Vector &targetPosition, const QAngle &targetAngles, bool tempDisableGravity, float timeOffset ) +{ + if ( tempDisableGravity != m_shadowTempGravityDisable ) + { + m_shadowTempGravityDisable = tempDisableGravity; + if ( !m_pShadow || m_pShadow->AllowsTranslation() ) + { + EnableGravity( !m_shadowTempGravityDisable ); + } + } + if ( m_pShadow ) + { + m_pShadow->Update( targetPosition, targetAngles, timeOffset ); + } +} + + +void CPhysicsObject::RemoveShadowController() +{ + if ( m_pShadow ) + { + CPhysicsEnvironment *pVEnv = GetVPhysicsEnvironment(); + pVEnv->DestroyShadowController( m_pShadow ); + m_pShadow = NULL; + } +} + +// Back door to allow save/restore of backlink between shadow controller and physics object +void CPhysicsObject::RestoreShadowController( IPhysicsShadowController *pShadowController ) +{ + Assert( !m_pShadow ); + m_pShadow = pShadowController; +} + +int CPhysicsObject::GetShadowPosition( Vector *position, QAngle *angles ) const +{ + IVP_U_Matrix matrix; + + IVP_Environment *pEnv = m_pObject->get_environment(); + double psi = pEnv->get_next_PSI_time().get_seconds(); + m_pObject->calc_at_matrix( psi, &matrix ); + if ( angles ) + { + ConvertRotationToHL( matrix, *angles ); + } + if ( position ) + { + ConvertPositionToHL( matrix.vv, *position ); + } + + return 1; +} + + +IPhysicsShadowController *CPhysicsObject::GetShadowController( void ) const +{ + return m_pShadow; +} + +const CPhysCollide *CPhysicsObject::GetCollide( void ) const +{ + return m_pCollide; +} + + +IVP_SurfaceManager *CPhysicsObject::GetSurfaceManager( void ) const +{ + if ( m_collideType != COLLIDE_BALL ) + { + return m_pObject->get_surface_manager(); + } + return NULL; +} + + +float CPhysicsObject::GetDragInDirection( const IVP_U_Float_Point &velocity ) const +{ + IVP_U_Float_Point local; + + const IVP_U_Matrix *m_world_f_core = m_pObject->get_core()->get_m_world_f_core_PSI(); + m_world_f_core->vimult3( &velocity, &local ); + + return m_dragCoefficient * IVP_Inline_Math::fabsd( local.k[0] * m_dragBasis.x ) + + IVP_Inline_Math::fabsd( local.k[1] * m_dragBasis.y ) + + IVP_Inline_Math::fabsd( local.k[2] * m_dragBasis.z ); +} + +float CPhysicsObject::GetAngularDragInDirection( const IVP_U_Float_Point &angVelocity ) const +{ + return m_angDragCoefficient * IVP_Inline_Math::fabsd( angVelocity.k[0] * m_angDragBasis.x ) + + IVP_Inline_Math::fabsd( angVelocity.k[1] * m_angDragBasis.y ) + + IVP_Inline_Math::fabsd( angVelocity.k[2] * m_angDragBasis.z ); +} + +const char *CPhysicsObject::GetName() const +{ + return m_pObject->get_name(); +} + +void CPhysicsObject::SetMaterialIndex( int materialIndex ) +{ + if ( m_materialIndex == materialIndex ) + return; + + m_materialIndex = materialIndex; + IVP_Material *pMaterial = physprops->GetIVPMaterial( materialIndex ); + Assert(pMaterial); + m_pObject->l_default_material = pMaterial; + m_callbacks |= CALLBACK_ENABLING_COLLISION; + BEGIN_IVP_ALLOCATION(); + m_pObject->recompile_material_changed(); + END_IVP_ALLOCATION(); + m_callbacks &= ~CALLBACK_ENABLING_COLLISION; + if ( GetShadowController() ) + { + GetShadowController()->ObjectMaterialChanged( materialIndex ); + } +} + +// convert square velocity magnitude from IVP to HL +float CPhysicsObject::GetEnergy() const +{ + IVP_Core *pCore = m_pObject->get_core(); + IVP_FLOAT energy = 0.0f; + IVP_U_Float_Point tmp; + + energy = 0.5f * pCore->get_mass() * pCore->speed.dot_product(&pCore->speed); // 1/2mvv + tmp.set_pairwise_mult(&pCore->rot_speed, pCore->get_rot_inertia()); // wI + energy += 0.5f * tmp.dot_product(&pCore->rot_speed); // 1/2mvv + 1/2wIw + + return ConvertEnergyToHL( energy ); +} + +float CPhysicsObject::ComputeShadowControl( const hlshadowcontrol_params_t ¶ms, float secondsToArrival, float dt ) +{ + return ComputeShadowControllerHL( this, params, secondsToArrival, dt ); +} + +float CPhysicsObject::GetSphereRadius() const +{ + if ( m_collideType != COLLIDE_BALL ) + return 0; + + return ConvertDistanceToHL( m_pObject->to_ball()->get_radius() ); +} + +float CPhysicsObject::CalculateLinearDrag( const Vector &unitDirection ) const +{ + IVP_U_Float_Point ivpDir; + ConvertDirectionToIVP( unitDirection, ivpDir ); + + return GetDragInDirection( ivpDir ); +} + +float CPhysicsObject::CalculateAngularDrag( const Vector &objectSpaceRotationAxis ) const +{ + IVP_U_Float_Point ivpAxis; + ConvertDirectionToIVP( objectSpaceRotationAxis, ivpAxis ); + + // drag factor is per-radian, convert to per-degree + return GetAngularDragInDirection( ivpAxis ) * DEG2RAD(1.0); +} + + +void CPhysicsObject::BecomeTrigger() +{ + if ( IsTrigger() ) + return; + + if ( GetShadowController() ) + { + // triggers won't have the standard collisions, so the material change is no longer necessary + // also: This will fix problems with surfaceprops if the trigger becomes a fluid. + GetShadowController()->UseShadowMaterial( false ); + } + EnableDrag( false ); + EnableGravity( false ); + + // UNDONE: Use defaults here? Do we want object sets by default? + IVP_Template_Phantom trigger; + trigger.manage_intruding_cores = IVP_TRUE; // manage a list of intruded objects + trigger.manage_sleeping_cores = IVP_TRUE; // don't untouch/touch on sleep/wake + trigger.dont_check_for_unmoveables = IVP_TRUE; + trigger.exit_policy_extra_radius = 0.1f; // relatively strict exit check [m] + + bool enableCollisions = IsCollisionEnabled(); + EnableCollisions( false ); + BEGIN_IVP_ALLOCATION(); + m_pObject->convert_to_phantom( &trigger ); + END_IVP_ALLOCATION(); + // hook up events + CPhysicsEnvironment *pVEnv = GetVPhysicsEnvironment(); + pVEnv->PhantomAdd( this ); + + + EnableCollisions( enableCollisions ); +} + + +void CPhysicsObject::RemoveTrigger() +{ + IVP_Controller_Phantom *pController = m_pObject->get_controller_phantom(); + + // NOTE: This will remove the back-link in the object + delete pController; +} + + +bool CPhysicsObject::IsTrigger() const +{ + return m_pObject->get_controller_phantom() != NULL ? true : false; +} + +bool CPhysicsObject::IsFluid() const +{ + IVP_Controller_Phantom *pController = m_pObject->get_controller_phantom(); + if ( pController ) + { + // UNDONE: Make a base class for triggers? IPhysicsTrigger? + // and derive fluids and any other triggers from that class + // then you can ask that class what to do here. + if ( pController->client_data ) + return true; + } + + return false; +} + +// sets the object to be hinged. Fixed it place, but able to rotate around one axis. +void CPhysicsObject::BecomeHinged( int localAxis ) +{ + if ( IsMoveable() ) + { + float savedMass = GetMass(); + + IVP_U_Float_Hesse *iri = (IVP_U_Float_Hesse *)m_pObject->get_core()->get_inv_rot_inertia(); + + float savedRI[3]; + for ( int i = 0; i < 3; i++ ) + savedRI[i] = iri->k[i]; + + SetMass( VPHYSICS_MAX_MASS ); + IVP_U_Float_Hesse tmp = *iri; +#if 0 + for ( i = 0; i < 3; i++ ) + tmp.k[i] = savedRI[i]; +#else + int localAxisIVP = ConvertCoordinateAxisToIVP(localAxis); + tmp.k[localAxisIVP] = savedRI[localAxisIVP]; +#endif + + SetMass( savedMass ); + *iri = tmp; + } + m_hingedAxis = localAxis+1; +} + +void CPhysicsObject::RemoveHinged() +{ + m_hingedAxis = 0; + m_pObject->get_core()->calc_calc(); +} + +// dumps info about the object to Msg() +void CPhysicsObject::OutputDebugInfo() const +{ + Msg("-----------------\nObject: %s\n", m_pObject->get_name()); + Msg("Mass: %.1f (inv %.3f)\n", GetMass(), GetInvMass() ); + Vector inertia = GetInertia(); + Vector invInertia = GetInvInertia(); + Msg("Inertia: %.2f, %.2f, %.2f (inv %.3f, %.3f, %.3f)\n", inertia.x, inertia.y, inertia.z, invInertia.x, invInertia.y, invInertia.z ); + + Vector speed, angSpeed; + GetVelocity( &speed, &angSpeed ); + Msg("Velocity: %.2f, %.2f, %.2f \n", speed.x, speed.y, speed.z ); + Msg("Ang Velocity: %.2f, %.2f, %.2f \n", angSpeed.x, angSpeed.y, angSpeed.z ); + + float damp, angDamp; + GetDamping( &damp, &angDamp ); + Msg("Damping %.2f linear, %.2f angular\n", damp, angDamp ); + + Msg("Linear Drag: %.2f, %.2f, %.2f (factor %.2f)\n", m_dragBasis.x, m_dragBasis.y, m_dragBasis.z, m_dragCoefficient ); + Msg("Angular Drag: %.2f, %.2f, %.2f (factor %.2f)\n", m_angDragBasis.x, m_angDragBasis.y, m_angDragBasis.z, m_angDragCoefficient ); + + if ( IsHinged() ) + { + const char *pAxisNames[] = {"x", "y", "z"}; + Msg("Hinged on %s axis\n", pAxisNames[m_hingedAxis-1] ); + } + Msg("attached to %d controllers\n", m_pObject->get_core()->controllers_of_core.len() ); + for (int k = m_pObject->get_core()->controllers_of_core.len()-1; k>=0;k--) + { + // NOTE: Set a breakpoint here and take a look at what it's hooked to + IVP_Controller *pController = m_pObject->get_core()->controllers_of_core.element_at(k); + Msg("%d) %s\n", k, pController->get_controller_name() ); + } + Msg("State: %s, Collision %s, Motion %s, %sFlags %04X (game %04x, index %d)\n", + IsAsleep() ? "Asleep" : "Awake", + IsCollisionEnabled() ? "Enabled" : "Disabled", + IsStatic() ? "Static" : (IsMotionEnabled() ? "Enabled" : "Disabled"), + (GetCallbackFlags() & CALLBACK_MARKED_FOR_TEST) ? "Debug! " : "", + (int)GetCallbackFlags(), (int)GetGameFlags(), (int)GetGameIndex() ); + + float matDensity = 0; + float matThickness = 0; + float matFriction = 0; + float matElasticity = 0; + physprops->GetPhysicsProperties( GetMaterialIndexInternal(), &matDensity, &matThickness, &matFriction, &matElasticity ); + Msg("Material: %s : density(%.1f), thickness(%.2f), friction(%.2f), elasticity(%.2f)\n", physprops->GetPropName(GetMaterialIndexInternal()), + matDensity, matThickness, matFriction, matElasticity ); + if ( GetCollide() ) + { + OutputCollideDebugInfo( GetCollide() ); + } +} + +bool CPhysicsObject::IsAttachedToConstraint( bool bExternalOnly ) const +{ + if ( m_pObject ) + { + for (int k = m_pObject->get_core()->controllers_of_core.len()-1; k>=0;k--) + { + IVP_Controller *pController = m_pObject->get_core()->controllers_of_core.element_at(k); + if ( pController->get_controller_priority() == IVP_CP_CONSTRAINTS ) + { + if ( !bExternalOnly || IsExternalConstraint(pController, GetGameData()) ) + return true; + } + } + } + return false; +} + +static void InitObjectTemplate( IVP_Template_Real_Object &objectTemplate, int materialIndex, objectparams_t *pParams, bool isStatic ) +{ + objectTemplate.mass = pParams->mass; + objectTemplate.mass = clamp( objectTemplate.mass, VPHYSICS_MIN_MASS, VPHYSICS_MAX_MASS ); + + if ( materialIndex >= 0 ) + { + objectTemplate.material = physprops->GetIVPMaterial( materialIndex ); + } + else + { + materialIndex = physprops->GetSurfaceIndex( "default" ); + objectTemplate.material = physprops->GetIVPMaterial( materialIndex ); + } + + // HACKHACK: Do something with this name? + BEGIN_IVP_ALLOCATION(); + if ( IsPC() ) + { + objectTemplate.set_name(pParams->pName); + } + END_IVP_ALLOCATION(); +#if USE_COLLISION_GROUP_STRING + objectTemplate.set_nocoll_group_ident( NULL ); +#endif + + objectTemplate.physical_unmoveable = isStatic ? IVP_TRUE : IVP_FALSE; + objectTemplate.rot_inertia_is_factor = IVP_TRUE; + + float inertia = pParams->inertia; + + // don't allow <=0 inertia!!!! + if ( inertia <= 0 ) + inertia = 1.0; + + if ( inertia > 1e18f ) + inertia = 1e18f; + + objectTemplate.rot_inertia.set(inertia, inertia, inertia); + objectTemplate.rot_speed_damp_factor.set(pParams->rotdamping, pParams->rotdamping, pParams->rotdamping); + objectTemplate.speed_damp_factor = pParams->damping; + objectTemplate.auto_check_rot_inertia = pParams->rotInertiaLimit; +} + +CPhysicsObject *CreatePhysicsObject( CPhysicsEnvironment *pEnvironment, const CPhysCollide *pCollisionModel, int materialIndex, const Vector &position, const QAngle& angles, objectparams_t *pParams, bool isStatic ) +{ + if ( materialIndex < 0 ) + { + materialIndex = physprops->GetSurfaceIndex( "default" ); + } + AssertOnce(materialIndex>=0 && materialIndex<127); + IVP_Template_Real_Object objectTemplate; + IVP_U_Quat rotation; + IVP_U_Point pos; + + Assert( position.IsValid() ); + Assert( angles.IsValid() ); + +#if _WIN32 + if ( !position.IsValid() || !angles.IsValid() ) + { + DebuggerBreakIfDebugging(); + Warning("Invalid initial position on %s\n", pParams->pName ); + + Vector *pPos = (Vector *)&position; + QAngle *pRot = (QAngle *)&angles; + if ( !pPos->IsValid() ) + pPos->Init(); + if ( !pRot->IsValid() ) + pRot->Init(); + } +#endif + + ConvertRotationToIVP( angles, rotation ); + ConvertPositionToIVP( position, pos ); + + InitObjectTemplate( objectTemplate, materialIndex, pParams, isStatic ); + + IVP_U_Matrix massCenterMatrix; + massCenterMatrix.init(); + if ( pParams->massCenterOverride ) + { + IVP_U_Point center; + ConvertPositionToIVP( *pParams->massCenterOverride, center ); + massCenterMatrix.shift_os( ¢er ); + objectTemplate.mass_center_override = &massCenterMatrix; + } + + CPhysicsObject *pObject = new CPhysicsObject(); + short collideType; + IVP_SurfaceManager *pSurman = CreateSurfaceManager( pCollisionModel, collideType ); + if ( !pSurman ) + return NULL; + pObject->m_collideType = collideType; + pObject->m_asleepSinceCreation = true; + + BEGIN_IVP_ALLOCATION(); + + IVP_Polygon *realObject = pEnvironment->GetIVPEnvironment()->create_polygon(pSurman, &objectTemplate, &rotation, &pos); + + pObject->Init( pCollisionModel, realObject, materialIndex, pParams->volume, pParams->dragCoefficient, pParams->dragCoefficient ); + pObject->SetGameData( pParams->pGameData ); + + if ( pParams->enableCollisions ) + { + pObject->EnableCollisions( true ); + } + if ( !isStatic && pParams->dragCoefficient != 0.0f ) + { + pObject->EnableDrag( true ); + } + + END_IVP_ALLOCATION(); + + return pObject; +} + +CPhysicsObject *CreatePhysicsSphere( CPhysicsEnvironment *pEnvironment, float radius, int materialIndex, const Vector &position, const QAngle &angles, objectparams_t *pParams, bool isStatic ) +{ + IVP_U_Quat rotation; + IVP_U_Point pos; + + ConvertRotationToIVP( angles, rotation ); + ConvertPositionToIVP( position, pos ); + + IVP_Template_Real_Object objectTemplate; + InitObjectTemplate( objectTemplate, materialIndex, pParams, isStatic ); + + IVP_Template_Ball ballTemplate; + ballTemplate.radius = ConvertDistanceToIVP( radius ); + + MEM_ALLOC_CREDIT(); + IVP_Ball *realObject = pEnvironment->GetIVPEnvironment()->create_ball( &ballTemplate, &objectTemplate, &rotation, &pos ); + + float volume = pParams->volume; + if ( volume <= 0 ) + { + volume = 4.0f * radius * radius * radius * M_PI / 3.0f; + } + CPhysicsObject *pObject = new CPhysicsObject(); + pObject->Init( NULL, realObject, materialIndex, volume, 0, 0 ); //, pParams->dragCoefficient, pParams->dragCoefficient + pObject->SetGameData( pParams->pGameData ); + + if ( pParams->enableCollisions ) + { + pObject->EnableCollisions( true ); + } + // drag is not supported on spheres + //pObject->EnableDrag( false ); + + return pObject; +} + +class CMaterialIndexOps : public CDefSaveRestoreOps +{ +public: + // save data type interface + virtual void Save( const SaveRestoreFieldInfo_t &fieldInfo, ISave *pSave ) + { + int materialIndex = *((int *)fieldInfo.pField); + const char *pMaterialName = physprops->GetPropName( materialIndex ); + if ( !pMaterialName ) + { + pMaterialName = physprops->GetPropName( 0 ); + } + int len = strlen(pMaterialName) + 1; + pSave->WriteInt( &len ); + pSave->WriteString( pMaterialName ); + } + + virtual void Restore( const SaveRestoreFieldInfo_t &fieldInfo, IRestore *pRestore ) + { + char nameBuf[1024]; + int nameLen = pRestore->ReadInt(); + pRestore->ReadString( nameBuf, sizeof(nameBuf), nameLen ); + int *pMaterialIndex = (int *)fieldInfo.pField; + *pMaterialIndex = physprops->GetSurfaceIndex( nameBuf ); + if ( *pMaterialIndex < 0 ) + { + *pMaterialIndex = 0; + } + } + + virtual bool IsEmpty( const SaveRestoreFieldInfo_t &fieldInfo ) + { + int *pMaterialIndex = (int *)fieldInfo.pField; + return (*pMaterialIndex == 0); + } + + virtual void MakeEmpty( const SaveRestoreFieldInfo_t &fieldInfo ) + { + int *pMaterialIndex = (int *)fieldInfo.pField; + *pMaterialIndex = 0; + } +}; + +static CMaterialIndexOps g_MaterialIndexDataOps; + +ISaveRestoreOps* MaterialIndexDataOps() +{ + return &g_MaterialIndexDataOps; +} + +BEGIN_SIMPLE_DATADESC( vphysics_save_cphysicsobject_t ) +// DEFINE_FIELD( pCollide, FIELD_??? ), // don't save this +// DEFINE_FIELD( pName, FIELD_??? ), // don't save this +DEFINE_FIELD( sphereRadius, FIELD_FLOAT ), +DEFINE_FIELD( isStatic, FIELD_BOOLEAN ), +DEFINE_FIELD( collisionEnabled, FIELD_BOOLEAN ), +DEFINE_FIELD( gravityEnabled, FIELD_BOOLEAN ), +DEFINE_FIELD( dragEnabled, FIELD_BOOLEAN ), +DEFINE_FIELD( motionEnabled, FIELD_BOOLEAN ), +DEFINE_FIELD( isAsleep, FIELD_BOOLEAN ), +DEFINE_FIELD( isTrigger, FIELD_BOOLEAN ), +DEFINE_FIELD( asleepSinceCreation, FIELD_BOOLEAN ), +DEFINE_FIELD( hasTouchedDynamic, FIELD_BOOLEAN ), +DEFINE_CUSTOM_FIELD( materialIndex, &g_MaterialIndexDataOps ), +DEFINE_FIELD( mass, FIELD_FLOAT ), +DEFINE_FIELD( rotInertia, FIELD_VECTOR ), +DEFINE_FIELD( speedDamping, FIELD_FLOAT ), +DEFINE_FIELD( rotSpeedDamping, FIELD_FLOAT ), +DEFINE_FIELD( massCenterOverride, FIELD_VECTOR ), +DEFINE_FIELD( callbacks, FIELD_INTEGER ), +DEFINE_FIELD( gameFlags, FIELD_INTEGER ), +DEFINE_FIELD( contentsMask, FIELD_INTEGER ), +DEFINE_FIELD( volume, FIELD_FLOAT ), +DEFINE_FIELD( dragCoefficient, FIELD_FLOAT ), +DEFINE_FIELD( angDragCoefficient, FIELD_FLOAT ), +DEFINE_FIELD( hasShadowController,FIELD_BOOLEAN ), +//DEFINE_VPHYSPTR( pShadow ), +DEFINE_FIELD( origin, FIELD_POSITION_VECTOR ), +DEFINE_FIELD( angles, FIELD_VECTOR ), +DEFINE_FIELD( velocity, FIELD_VECTOR ), +DEFINE_FIELD( angVelocity, FIELD_VECTOR ), +DEFINE_FIELD( collideType, FIELD_SHORT ), +DEFINE_FIELD( gameIndex, FIELD_SHORT ), +DEFINE_FIELD( hingeAxis, FIELD_INTEGER ), +END_DATADESC() + +bool CPhysicsObject::IsCollisionEnabled() const +{ + return GetObject()->is_collision_detection_enabled() ? true : false; +} + +void CPhysicsObject::WriteToTemplate( vphysics_save_cphysicsobject_t &objectTemplate ) +{ + if ( m_collideType == COLLIDE_BALL ) + { + objectTemplate.pCollide = NULL; + objectTemplate.sphereRadius = GetSphereRadius(); + } + else + { + objectTemplate.pCollide = GetCollide(); + objectTemplate.sphereRadius = 0; + } + objectTemplate.isStatic = IsStatic(); + objectTemplate.collisionEnabled = IsCollisionEnabled(); + objectTemplate.gravityEnabled = IsGravityEnabled(); + objectTemplate.dragEnabled = IsDragEnabled(); + objectTemplate.motionEnabled = IsMotionEnabled(); + objectTemplate.isAsleep = IsAsleep(); + objectTemplate.isTrigger = IsTrigger(); + objectTemplate.asleepSinceCreation = m_asleepSinceCreation; + objectTemplate.materialIndex = m_materialIndex; + objectTemplate.mass = GetMass(); + + objectTemplate.rotInertia = GetInertia(); + GetDamping( &objectTemplate.speedDamping, &objectTemplate.rotSpeedDamping ); + objectTemplate.massCenterOverride.Init(); + if ( !IsMassCenterAtDefault() ) + { + objectTemplate.massCenterOverride = GetMassCenterLocalSpace(); + } + + objectTemplate.callbacks = m_callbacks; + objectTemplate.gameFlags = m_gameFlags; + objectTemplate.volume = GetVolume(); + objectTemplate.dragCoefficient = m_dragCoefficient; + objectTemplate.angDragCoefficient = m_angDragCoefficient; + objectTemplate.pShadow = m_pShadow; + objectTemplate.hasShadowController = (m_pShadow != NULL) ? true : false; + objectTemplate.hasTouchedDynamic = HasTouchedDynamic(); + //bool m_shadowTempGravityDisable; + objectTemplate.collideType = m_collideType; + objectTemplate.gameIndex = m_gameIndex; + objectTemplate.contentsMask = m_contentsMask; + objectTemplate.hingeAxis = m_hingedAxis; + GetPosition( &objectTemplate.origin, &objectTemplate.angles ); + GetVelocity( &objectTemplate.velocity, &objectTemplate.angVelocity ); +} + +void CPhysicsObject::InitFromTemplate( CPhysicsEnvironment *pEnvironment, void *pGameData, const vphysics_save_cphysicsobject_t &objectTemplate ) +{ + MEM_ALLOC_CREDIT(); + m_collideType = objectTemplate.collideType; + + IVP_Template_Real_Object ivpObjectTemplate; + IVP_U_Quat rotation; + IVP_U_Point pos; + + ConvertRotationToIVP( objectTemplate.angles, rotation ); + ConvertPositionToIVP( objectTemplate.origin, pos ); + + ivpObjectTemplate.mass = objectTemplate.mass; + + if ( objectTemplate.materialIndex >= 0 ) + { + ivpObjectTemplate.material = physprops->GetIVPMaterial( objectTemplate.materialIndex ); + } + else + { + ivpObjectTemplate.material = physprops->GetIVPMaterial( physprops->GetSurfaceIndex( "default" ) ); + } + + Assert( ivpObjectTemplate.material ); + // HACKHACK: Pass this name in for debug + ivpObjectTemplate.set_name(objectTemplate.pName); +#if USE_COLLISION_GROUP_STRING + ivpObjectTemplate.set_nocoll_group_ident( NULL ); +#endif + + ivpObjectTemplate.physical_unmoveable = objectTemplate.isStatic ? IVP_TRUE : IVP_FALSE; + ivpObjectTemplate.rot_inertia_is_factor = IVP_TRUE; + + ivpObjectTemplate.rot_inertia.set( 1,1,1 ); + ivpObjectTemplate.rot_speed_damp_factor.set( objectTemplate.rotSpeedDamping, objectTemplate.rotSpeedDamping, objectTemplate.rotSpeedDamping ); + ivpObjectTemplate.speed_damp_factor = objectTemplate.speedDamping; + + IVP_U_Matrix massCenterMatrix; + massCenterMatrix.init(); + if ( objectTemplate.massCenterOverride != vec3_origin ) + { + IVP_U_Point center; + ConvertPositionToIVP( objectTemplate.massCenterOverride, center ); + massCenterMatrix.shift_os( ¢er ); + ivpObjectTemplate.mass_center_override = &massCenterMatrix; + } + + IVP_Real_Object *realObject = NULL; + if ( m_collideType == COLLIDE_BALL ) + { + IVP_Template_Ball ballTemplate; + ballTemplate.radius = ConvertDistanceToIVP( objectTemplate.sphereRadius ); + + realObject = pEnvironment->GetIVPEnvironment()->create_ball( &ballTemplate, &ivpObjectTemplate, &rotation, &pos ); + } + else + { + short collideType; + IVP_SurfaceManager *surman = CreateSurfaceManager( objectTemplate.pCollide, collideType ); + m_collideType = collideType; + realObject = pEnvironment->GetIVPEnvironment()->create_polygon(surman, &ivpObjectTemplate, &rotation, &pos); + } + + m_pObject = realObject; + SetInertia( objectTemplate.rotInertia ); + Init( objectTemplate.pCollide, realObject, objectTemplate.materialIndex, objectTemplate.volume, objectTemplate.dragCoefficient, objectTemplate.dragCoefficient ); + + SetCallbackFlags( (unsigned short) objectTemplate.callbacks ); + SetGameFlags( (unsigned short) objectTemplate.gameFlags ); + SetGameIndex( objectTemplate.gameIndex ); + SetGameData( pGameData ); + SetContents( objectTemplate.contentsMask ); + + if ( objectTemplate.dragEnabled ) + { + Assert( !objectTemplate.isStatic ); + EnableDrag( true ); + } + + if ( !objectTemplate.motionEnabled ) + { + Assert( !objectTemplate.isStatic ); + EnableMotion( false ); + } + + if ( objectTemplate.isTrigger ) + { + BecomeTrigger(); + } + + if ( !objectTemplate.gravityEnabled ) + { + EnableGravity( false ); + } + + if ( objectTemplate.collisionEnabled ) + { + EnableCollisions( true ); + } + + // will wake up the object + if ( objectTemplate.velocity.LengthSqr() != 0 || objectTemplate.angVelocity.LengthSqr() != 0 ) + { + SetVelocityInstantaneous( &objectTemplate.velocity, &objectTemplate.angVelocity ); + if ( objectTemplate.isAsleep ) + { + Sleep(); + } + } + + m_asleepSinceCreation = objectTemplate.asleepSinceCreation; + if ( !objectTemplate.isAsleep ) + { + Assert( !objectTemplate.isStatic ); + Wake(); + } + + if ( objectTemplate.hingeAxis ) + { + BecomeHinged( objectTemplate.hingeAxis-1 ); + } + if ( objectTemplate.hasTouchedDynamic ) + { + SetTouchedDynamic(); + } + + m_pShadow = NULL; +} + + +bool SavePhysicsObject( const physsaveparams_t ¶ms, CPhysicsObject *pObject ) +{ + vphysics_save_cphysicsobject_t objectTemplate; + memset( &objectTemplate, 0, sizeof(objectTemplate) ); + + pObject->WriteToTemplate( objectTemplate ); + params.pSave->WriteAll( &objectTemplate ); + + if ( objectTemplate.hasShadowController ) + { + return SavePhysicsShadowController( params, objectTemplate.pShadow ); + } + return true; +} + +bool RestorePhysicsObject( const physrestoreparams_t ¶ms, CPhysicsObject **ppObject ) +{ + vphysics_save_cphysicsobject_t objectTemplate; + memset( &objectTemplate, 0, sizeof(objectTemplate) ); + params.pRestore->ReadAll( &objectTemplate ); + Assert(objectTemplate.origin.IsValid()); + Assert(objectTemplate.angles.IsValid()); + objectTemplate.pCollide = params.pCollisionModel; + objectTemplate.pName = params.pName; + *ppObject = new CPhysicsObject(); + + postrestore_objectlist_t entry; + entry.Defaults(); + + if ( objectTemplate.collisionEnabled ) + { + // queue up the collision enable for these in case their entities have other dependent + // physics handlers (like controllers) that need to be restored before callbacks are useful + entry.pObject = *ppObject; + entry.enableCollisions = true; + objectTemplate.collisionEnabled = false; + } + + (*ppObject)->InitFromTemplate( static_cast<CPhysicsEnvironment *>(params.pEnvironment), params.pGameData, objectTemplate ); + + if ( (*ppObject)->IsAsleep() && !(*ppObject)->m_asleepSinceCreation && !(*ppObject)->IsStatic() ) + { + entry.pObject = *ppObject; + entry.growFriction = true; + } + + if ( entry.pObject ) + { + g_PostRestoreObjectList.AddToTail( entry ); + } + + if ( objectTemplate.hasShadowController ) + { + bool restored = RestorePhysicsShadowControllerInternal( params, &objectTemplate.pShadow, *ppObject ); + (*ppObject)->RestoreShadowController( objectTemplate.pShadow ); + return restored; + } + + return true; +} + +IPhysicsObject *CreateObjectFromBuffer( CPhysicsEnvironment *pEnvironment, void *pGameData, unsigned char *pBuffer, unsigned int bufferSize, bool enableCollisions ) +{ + CPhysicsObject *pObject = new CPhysicsObject(); + if ( bufferSize >= sizeof(vphysics_save_cphysicsobject_t)) + { + vphysics_save_cphysicsobject_t *pTemplate = reinterpret_cast<vphysics_save_cphysicsobject_t *>(pBuffer); + pTemplate->hasShadowController = false; // this hasn't been saved separately so cannot be supported via this path + pObject->InitFromTemplate( pEnvironment, pGameData, *pTemplate ); + if ( pTemplate->collisionEnabled && enableCollisions ) + { + pObject->EnableCollisions(true); + } + return pObject; + } + return NULL; +} + +IPhysicsObject *CreateObjectFromBuffer_UseExistingMemory( CPhysicsEnvironment *pEnvironment, void *pGameData, unsigned char *pBuffer, unsigned int bufferSize, CPhysicsObject *pExistingMemory ) +{ + if ( bufferSize >= sizeof(vphysics_save_cphysicsobject_t)) + { + vphysics_save_cphysicsobject_t *pTemplate = reinterpret_cast<vphysics_save_cphysicsobject_t *>(pBuffer); + // Allow the placement new. If we don't do this, then it'll get a compile error because new + // might be defined as the special form in MEMALL_DEBUG_NEW. + #include "tier0/memdbgoff.h" + pExistingMemory = new ( pExistingMemory ) CPhysicsObject(); + #include "tier0/memdbgon.h" + pExistingMemory->InitFromTemplate( pEnvironment, pGameData, *pTemplate ); + if ( pTemplate->collisionEnabled ) + { + pExistingMemory->EnableCollisions(true); + } + return pExistingMemory; + } + return NULL; +} + +// regenerate the friction systems for these objects. Because when it was saved it had them (came to rest with the contact points). +// So now we need to recreate them or some objects may not wake up when this object (or its neighbors) are deleted. +void PostRestorePhysicsObject() +{ + for ( int i = g_PostRestoreObjectList.Count()-1; i >= 0; --i ) + { + if ( g_PostRestoreObjectList[i].pObject ) + { + if ( g_PostRestoreObjectList[i].growFriction ) + { + g_PostRestoreObjectList[i].pObject->GetObject()->force_grow_friction_system(); + } + if ( g_PostRestoreObjectList[i].enableCollisions ) + { + g_PostRestoreObjectList[i].pObject->EnableCollisions( true ); + } + } + } + g_PostRestoreObjectList.Purge(); +} diff --git a/vphysics/physics_object.h b/vphysics/physics_object.h new file mode 100644 index 0000000..a1c6f5e --- /dev/null +++ b/vphysics/physics_object.h @@ -0,0 +1,287 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_OBJECT_H +#define PHYSICS_OBJECT_H + +#ifdef _WIN32 +#pragma once +#endif + +#include "vphysics_interface.h" + +class IVP_Real_Object; +class IVP_Environment; +class IVP_U_Float_Point; +class IVP_SurfaceManager; +class IVP_Controller; +class CPhysicsEnvironment; +struct vphysics_save_cphysicsobject_t +{ + const CPhysCollide *pCollide; + const char *pName; + float sphereRadius; + + bool isStatic; + bool collisionEnabled; + bool gravityEnabled; + bool dragEnabled; + bool motionEnabled; + bool isAsleep; + bool isTrigger; + bool asleepSinceCreation; // has this been asleep since creation? + bool hasTouchedDynamic; + bool hasShadowController; + short collideType; + unsigned short gameIndex; + int hingeAxis; + int materialIndex; + float mass; + Vector rotInertia; + float speedDamping; + float rotSpeedDamping; + Vector massCenterOverride; + + unsigned int callbacks; + unsigned int gameFlags; + + unsigned int contentsMask; + + float volume; + float dragCoefficient; + float angDragCoefficient; + IPhysicsShadowController *pShadow; + //bool m_shadowTempGravityDisable; + + Vector origin; + QAngle angles; + Vector velocity; + AngularImpulse angVelocity; + + DECLARE_SIMPLE_DATADESC(); +}; + +enum +{ + OBJ_AWAKE = 0, // awake, simulating + OBJ_STARTSLEEP = 1, // going to sleep, but not queried yet + OBJ_SLEEP = 2, // sleeping, no state changes since last query +}; + + +class CPhysicsObject : public IPhysicsObject +{ +public: + CPhysicsObject( void ); + virtual ~CPhysicsObject( void ); + + void Init( const CPhysCollide *pCollisionModel, IVP_Real_Object *pObject, int materialIndex, float volume, float drag, float angDrag ); + + // IPhysicsObject functions + bool IsStatic() const; + bool IsAsleep() const; + bool IsTrigger() const; + bool IsFluid() const; + bool IsHinged() const { return (m_hingedAxis != 0) ? true : false; } + bool IsCollisionEnabled() const; + bool IsGravityEnabled() const; + bool IsDragEnabled() const; + bool IsMotionEnabled() const; + bool IsMoveable() const; + bool IsAttachedToConstraint( bool bExternalOnly ) const; + + + void EnableCollisions( bool enable ); + // Enable / disable gravity for this object + void EnableGravity( bool enable ); + // Enable / disable air friction / drag for this object + void EnableDrag( bool enable ); + void EnableMotion( bool enable ); + + void SetGameData( void *pAppData ); + void *GetGameData( void ) const; + void SetCallbackFlags( unsigned short callbackflags ); + unsigned short GetCallbackFlags( void ) const; + void SetGameFlags( unsigned short userFlags ); + unsigned short GetGameFlags( void ) const; + void SetGameIndex( unsigned short gameIndex ); + unsigned short GetGameIndex( void ) const; + + void Wake(); + void Sleep(); + void RecheckCollisionFilter(); + void RecheckContactPoints(); + + void SetMass( float mass ); + float GetMass( void ) const; + float GetInvMass( void ) const; + void SetInertia( const Vector &inertia ); + Vector GetInertia( void ) const; + Vector GetInvInertia( void ) const; + + void GetDamping( float *speed, float *rot ) const; + void SetDamping( const float *speed, const float *rot ); + void SetDragCoefficient( float *pDrag, float *pAngularDrag ); + void SetBuoyancyRatio( float ratio ); + int GetMaterialIndex() const { return GetMaterialIndexInternal(); } + void SetMaterialIndex( int materialIndex ); + inline int GetMaterialIndexInternal( void ) const { return m_materialIndex; } + + unsigned int GetContents() const { return m_contentsMask; } + void SetContents( unsigned int contents ); + + float GetSphereRadius() const; + Vector GetMassCenterLocalSpace() const; + float GetEnergy() const; + + void SetPosition( const Vector &worldPosition, const QAngle &angles, bool isTeleport = false ); + void SetPositionMatrix( const matrix3x4_t& matrix, bool isTeleport = false ); + void GetPosition( Vector *worldPosition, QAngle *angles ) const; + void GetPositionMatrix( matrix3x4_t *positionMatrix ) const; + + void SetVelocity( const Vector *velocity, const AngularImpulse *angularVelocity ); + void SetVelocityInstantaneous( const Vector *velocity, const AngularImpulse *angularVelocity ); + void AddVelocity( const Vector *velocity, const AngularImpulse *angularVelocity ); + void GetVelocity( Vector *velocity, AngularImpulse *angularVelocity ) const; + void GetImplicitVelocity( Vector *velocity, AngularImpulse *angularVelocity ) const; + void GetVelocityAtPoint( const Vector &worldPosition, Vector *pVelocity ) const; + + void LocalToWorld( Vector *worldPosition, const Vector &localPosition ) const; + void WorldToLocal( Vector *localPosition, const Vector &worldPosition ) const; + void LocalToWorldVector( Vector *worldVector, const Vector &localVector ) const; + void WorldToLocalVector( Vector *localVector, const Vector &worldVector ) const; + + void ApplyForceCenter( const Vector &forceVector ); + void ApplyForceOffset( const Vector &forceVector, const Vector &worldPosition ); + void ApplyTorqueCenter( const AngularImpulse & ); + void CalculateForceOffset( const Vector &forceVector, const Vector &worldPosition, Vector *centerForce, AngularImpulse *centerTorque ) const; + void CalculateVelocityOffset( const Vector &forceVector, const Vector &worldPosition, Vector *centerVelocity, AngularImpulse *centerAngularVelocity ) const; + float CalculateLinearDrag( const Vector &unitDirection ) const; + float CalculateAngularDrag( const Vector &objectSpaceRotationAxis ) const; + + bool GetContactPoint( Vector *contactPoint, IPhysicsObject **contactObject ) const; + void SetShadow( float maxSpeed, float maxAngularSpeed, bool allowPhysicsMovement, bool allowPhysicsRotation ); + void UpdateShadow( const Vector &targetPosition, const QAngle &targetAngles, bool tempDisableGravity, float timeOffset ); + void RemoveShadowController(); + int GetShadowPosition( Vector *position, QAngle *angles ) const; + IPhysicsShadowController *GetShadowController( void ) const; + float ComputeShadowControl( const hlshadowcontrol_params_t ¶ms, float secondsToArrival, float dt ); + + const CPhysCollide *GetCollide( void ) const; + char const *GetName() const; + + float GetDragInDirection( const IVP_U_Float_Point &dir ) const; + float GetAngularDragInDirection( const IVP_U_Float_Point &angVelocity ) const; + void BecomeTrigger(); + void RemoveTrigger(); + void BecomeHinged( int localAxis ); + void RemoveHinged(); + + IPhysicsFrictionSnapshot *CreateFrictionSnapshot(); + void DestroyFrictionSnapshot( IPhysicsFrictionSnapshot *pSnapshot ); + + void OutputDebugInfo() const; + + // local functions + inline IVP_Real_Object *GetObject( void ) const { return m_pObject; } + inline int CallbackFlags( void ) const { return m_callbacks; } + inline void AddCallbackFlags( unsigned short flags ) { m_callbacks |= flags; } + inline void RemoveCallbackFlags( unsigned short flags ) { m_callbacks &= ~flags; } + inline bool HasTouchedDynamic(); + inline void SetTouchedDynamic(); + void NotifySleep( void ); + void NotifyWake( void ); + int GetSleepState( void ) const { return m_sleepState; } + inline void ForceSilentDelete() { m_forceSilentDelete = true; } + + inline int GetActiveIndex( void ) const { return m_activeIndex; } + inline void SetActiveIndex( int index ) { m_activeIndex = index; } + inline float GetBuoyancyRatio( void ) const { return m_buoyancyRatio; } + // returns true if the mass center is set to the default for the collision model + bool IsMassCenterAtDefault() const; + + // is this object simulated, or controlled by game logic? + bool IsControlledByGame() const; + + IVP_SurfaceManager *GetSurfaceManager( void ) const; + + void WriteToTemplate( vphysics_save_cphysicsobject_t &objectTemplate ); + void InitFromTemplate( CPhysicsEnvironment *pEnvironment, void *pGameData, const vphysics_save_cphysicsobject_t &objectTemplate ); + + CPhysicsEnvironment *GetVPhysicsEnvironment(); + const CPhysicsEnvironment *GetVPhysicsEnvironment() const; + +private: + // NOTE: Local to vphysics, used to save/restore shadow controller + void RestoreShadowController( IPhysicsShadowController *pShadowController ); + friend bool RestorePhysicsObject( const physrestoreparams_t ¶ms, CPhysicsObject **ppObject ); + + bool IsControlling( const IVP_Controller *pController ) const; + float GetVolume() const; + void SetVolume( float volume ); + + // the mass has changed, recompute the drag information + void RecomputeDragBases(); + + void ClampVelocity(); + + // NOTE: If m_pGameData is not the first member, the constructor debug code must be modified + void *m_pGameData; + IVP_Real_Object *m_pObject; + const CPhysCollide *m_pCollide; + IPhysicsShadowController *m_pShadow; + + Vector m_dragBasis; + Vector m_angDragBasis; + + // these 5 should pack into a short + // pack new bools here + bool m_shadowTempGravityDisable : 5; + bool m_hasTouchedDynamic : 1; + bool m_asleepSinceCreation : 1; + bool m_forceSilentDelete : 1; + unsigned char m_sleepState : 2; + unsigned char m_hingedAxis : 3; + unsigned char m_collideType : 3; + unsigned short m_gameIndex; + +private: + unsigned short m_materialIndex; + unsigned short m_activeIndex; + + unsigned short m_callbacks; + unsigned short m_gameFlags; + unsigned int m_contentsMask; + + float m_volume; + float m_buoyancyRatio; + float m_dragCoefficient; + float m_angDragCoefficient; + + friend CPhysicsObject *CreatePhysicsObject( CPhysicsEnvironment *pEnvironment, const CPhysCollide *pCollisionModel, int materialIndex, const Vector &position, const QAngle& angles, objectparams_t *pParams, bool isStatic ); + friend bool CPhysicsEnvironment::TransferObject( IPhysicsObject *pObject, IPhysicsEnvironment *pDestinationEnvironment ); //need direct access to m_pShadow for Portal mod's physics object transfer system +}; + +// If you haven't ever touched a dynamic object, there's no need to search for contacting objects to +// wakeup when you are deleted. So cache a bit here when contacts are generated +inline bool CPhysicsObject::HasTouchedDynamic() +{ + return m_hasTouchedDynamic; +} + +inline void CPhysicsObject::SetTouchedDynamic() +{ + m_hasTouchedDynamic = true; +} + +extern CPhysicsObject *CreatePhysicsObject( CPhysicsEnvironment *pEnvironment, const CPhysCollide *pCollisionModel, int materialIndex, const Vector &position, const QAngle &angles, objectparams_t *pParams, bool isStatic ); +extern CPhysicsObject *CreatePhysicsSphere( CPhysicsEnvironment *pEnvironment, float radius, int materialIndex, const Vector &position, const QAngle &angles, objectparams_t *pParams, bool isStatic ); +extern void PostRestorePhysicsObject(); +extern IPhysicsObject *CreateObjectFromBuffer( CPhysicsEnvironment *pEnvironment, void *pGameData, unsigned char *pBuffer, unsigned int bufferSize, bool enableCollisions ); +extern IPhysicsObject *CreateObjectFromBuffer_UseExistingMemory( CPhysicsEnvironment *pEnvironment, void *pGameData, unsigned char *pBuffer, unsigned int bufferSize, CPhysicsObject *pExistingMemory ); + +#endif // PHYSICS_OBJECT_H diff --git a/vphysics/physics_shadow.cpp b/vphysics/physics_shadow.cpp new file mode 100644 index 0000000..aba3452 --- /dev/null +++ b/vphysics/physics_shadow.cpp @@ -0,0 +1,1420 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" + +#include "physics_shadow.h" +#include "vphysics/player_controller.h" +#include "physics_friction.h" +#include "vphysics/friction.h" + +// IsInContact +#include "ivp_mindist.hxx" +#include "ivp_core.hxx" +#include "ivp_friction.hxx" +#include "ivp_listener_object.hxx" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +struct vphysics_save_cshadowcontroller_t; +struct vphysics_save_shadowcontrolparams_t; + + +// UNDONE: Try this controller! +//damping is usually 1.0 +//frequency is usually in the range 1..16 +void ComputePDControllerCoefficients( float *coefficientsOut, const float frequency, const float damping, const float dt ) +{ + const float ks = 9.0f * frequency * frequency; + const float kd = 4.5f * frequency * damping; + + const float scale = 1.0f / ( 1.0f + kd * dt + ks * dt * dt ); + + coefficientsOut[0] = ks * scale; + coefficientsOut[1] = ( kd + ks * dt ) * scale; + + // Use this controller like: + // speed += (coefficientsOut[0] * (targetPos - currentPos) + coefficientsOut[1] * (targetSpeed - currentSpeed)) * dt +} + +void ComputeController( IVP_U_Float_Point ¤tSpeed, const IVP_U_Float_Point &delta, float maxSpeed, float maxDampSpeed, float scaleDelta, float damping, IVP_U_Float_Point *pOutImpulse = NULL ) +{ + if ( currentSpeed.quad_length() < 1e-6 ) + { + currentSpeed.set_to_zero(); + } + + // scale by timestep + IVP_U_Float_Point acceleration; + if ( maxSpeed > 0 ) + { + acceleration.set_multiple( &delta, scaleDelta ); + float speed = acceleration.real_length(); + if ( speed > maxSpeed ) + { + speed = maxSpeed / speed; + acceleration.mult( speed ); + } + } + else + { + acceleration.set_to_zero(); + } + + IVP_U_Float_Point dampAccel; + if ( maxDampSpeed > 0 ) + { + dampAccel.set_multiple( ¤tSpeed, -damping ); + float speed = dampAccel.real_length(); + if ( speed > maxDampSpeed ) + { + speed = maxDampSpeed / speed; + dampAccel.mult( speed ); + } + } + else + { + dampAccel.set_to_zero(); + } + currentSpeed.add( &dampAccel ); + currentSpeed.add( &acceleration ); + if ( pOutImpulse ) + { + *pOutImpulse = acceleration; + } +} + + +void ComputeController( IVP_U_Float_Point ¤tSpeed, const IVP_U_Float_Point &delta, const IVP_U_Float_Point &maxSpeed, float scaleDelta, float damping, IVP_U_Float_Point *pOutImpulse ) +{ + // scale by timestep + IVP_U_Float_Point acceleration; + acceleration.set_multiple( &delta, scaleDelta ); + + if ( currentSpeed.quad_length() < 1e-6 ) + { + currentSpeed.set_to_zero(); + } + + acceleration.add_multiple( ¤tSpeed, -damping ); + + for(int i=2; i>=0; i--) + { + if(IVP_Inline_Math::fabsd(acceleration.k[i]) < maxSpeed.k[i]) + continue; + + // clip force + acceleration.k[i] = (acceleration.k[i] < 0) ? -maxSpeed.k[i] : maxSpeed.k[i]; + } + + currentSpeed.add( &acceleration ); + if ( pOutImpulse ) + { + *pOutImpulse = acceleration; + } +} + + +static bool IsOnGround( IVP_Real_Object *pivp ) +{ + IPhysicsFrictionSnapshot *pSnapshot = CreateFrictionSnapshot( pivp ); + bool bGround = false; + while (pSnapshot->IsValid()) + { + Vector normal; + pSnapshot->GetSurfaceNormal( normal ); + if ( normal.z < -0.7f ) + { + bGround = true; + break; + } + + pSnapshot->NextFrictionData(); + } + DestroyFrictionSnapshot( pSnapshot ); + return bGround; +} + +class CPlayerController : public IVP_Controller_Independent, public IPhysicsPlayerController, public IVP_Listener_Object +{ +public: + CPlayerController( CPhysicsObject *pObject ); + ~CPlayerController( void ); + + // ipion interfaces + void do_simulation_controller( IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> *cores); + virtual IVP_CONTROLLER_PRIORITY get_controller_priority() { return (IVP_CONTROLLER_PRIORITY) (IVP_CP_MOTION+1); } + virtual const char *get_controller_name() { return "vphysics:player"; } + + void SetObject( IPhysicsObject *pObject ); + void SetEventHandler( IPhysicsPlayerControllerEvent *handler ); + void Update( const Vector& position, const Vector& velocity, float secondsToArrival, bool onground, IPhysicsObject *ground ); + void MaxSpeed( const Vector &velocity ); + bool IsInContact( void ); + virtual bool WasFrozen() + { + IVP_Real_Object *pivp = m_pObject->GetObject(); + IVP_Core *pCore = pivp->get_core(); + return pCore->temporarily_unmovable ? true : false; + } + + void ForceTeleportToCurrentPosition() + { + m_forceTeleport = true; + } + + int GetShadowPosition( Vector *position, QAngle *angles ) + { + IVP_U_Matrix matrix; + + IVP_Environment *pEnv = m_pObject->GetObject()->get_environment(); + + double psi = pEnv->get_next_PSI_time().get_seconds(); + m_pObject->GetObject()->calc_at_matrix( psi, &matrix ); + if ( angles ) + { + ConvertRotationToHL( matrix, *angles ); + } + if ( position ) + { + ConvertPositionToHL( matrix.vv, *position ); + } + + return 1; + } + void GetShadowVelocity( Vector *velocity ); + virtual void GetLastImpulse( Vector *pOut ) + { + ConvertPositionToHL( m_lastImpulse, *pOut ); + } + + virtual void StepUp( float height ); + virtual void Jump(); + virtual IPhysicsObject *GetObject() { return m_pObject; } + + virtual void SetPushMassLimit( float maxPushMass ) + { + m_pushableMassLimit = maxPushMass; + } + + virtual void SetPushSpeedLimit( float maxPushSpeed ) + { + m_pushableSpeedLimit = maxPushSpeed; + } + + virtual float GetPushMassLimit() { return m_pushableMassLimit; } + virtual float GetPushSpeedLimit() { return m_pushableSpeedLimit; } + + // Object listener + virtual void event_object_deleted( IVP_Event_Object *pEvent) + { + Assert( pEvent->real_object == m_pGround->GetObject() ); + m_pGround = NULL; + } + virtual void event_object_created( IVP_Event_Object *) {} + virtual void event_object_revived( IVP_Event_Object *) {} + virtual void event_object_frozen ( IVP_Event_Object *) {} + +private: + void AttachObject( void ); + void DetachObject( void ); + int TryTeleportObject( void ); + void SetGround( CPhysicsObject *pGroundObject ); + + CPhysicsObject *m_pObject; + IVP_U_Float_Point m_saveRot; + CPhysicsObject *m_pGround; // Uses object listener to clear - so ok to hold over frames + + IPhysicsPlayerControllerEvent *m_handler; + float m_maxDeltaPosition; + float m_dampFactor; + float m_secondsToArrival; + float m_pushableMassLimit; + float m_pushableSpeedLimit; + IVP_U_Point m_targetPosition; + IVP_U_Float_Point m_groundPosition; + IVP_U_Float_Point m_maxSpeed; + IVP_U_Float_Point m_currentSpeed; + IVP_U_Float_Point m_lastImpulse; + bool m_enable : 1; + bool m_onground : 1; + bool m_forceTeleport : 1; + bool m_updatedSinceLast : 5; +}; + + +CPlayerController::CPlayerController( CPhysicsObject *pObject ) +{ + m_pGround = NULL; + m_pObject = pObject; + m_handler = NULL; + m_maxDeltaPosition = ConvertDistanceToIVP( 24 ); + m_dampFactor = 1.0f; + m_targetPosition.k[0] = m_targetPosition.k[1] = m_targetPosition.k[2] = 0; + m_pushableMassLimit = VPHYSICS_MAX_MASS; + m_pushableSpeedLimit = 1e4f; + m_forceTeleport = false; + AttachObject(); +} + +CPlayerController::~CPlayerController( void ) +{ + DetachObject(); +} + +void CPlayerController::SetGround( CPhysicsObject *pGroundObject ) +{ + if ( m_pGround != pGroundObject ) + { + if ( m_pGround && m_pGround->GetObject() ) + { + m_pGround->GetObject()->remove_listener_object(this); + } + m_pGround = pGroundObject; + if ( m_pGround && m_pGround->GetObject() ) + { + m_pGround->GetObject()->add_listener_object(this); + } + } +} + +void CPlayerController::AttachObject( void ) +{ + m_pObject->EnableDrag( false ); + IVP_Real_Object *pivp = m_pObject->GetObject(); + IVP_Core *pCore = pivp->get_core(); + m_saveRot = pCore->rot_speed_damp_factor; + pCore->rot_speed_damp_factor = IVP_U_Float_Point( 100, 100, 100 ); + pCore->calc_calc(); + BEGIN_IVP_ALLOCATION(); + pivp->get_environment()->get_controller_manager()->add_controller_to_core( this, pCore ); + END_IVP_ALLOCATION(); + m_pObject->AddCallbackFlags( CALLBACK_IS_PLAYER_CONTROLLER ); +} + +void CPlayerController::DetachObject( void ) +{ + if ( !m_pObject ) + return; + + IVP_Real_Object *pivp = m_pObject->GetObject(); + IVP_Core *pCore = pivp->get_core(); + pCore->rot_speed_damp_factor = m_saveRot; + pCore->calc_calc(); + m_pObject->RemoveCallbackFlags( CALLBACK_IS_PLAYER_CONTROLLER ); + m_pObject = NULL; + pivp->get_environment()->get_controller_manager()->remove_controller_from_core( this, pCore ); + SetGround(NULL); +} + +void CPlayerController::SetObject( IPhysicsObject *pObject ) +{ + CPhysicsObject *obj = (CPhysicsObject *)pObject; + if ( obj == m_pObject ) + return; + + DetachObject(); + m_pObject = obj; + AttachObject(); +} + +int CPlayerController::TryTeleportObject( void ) +{ + if ( m_handler && !m_forceTeleport ) + { + Vector hlPosition; + ConvertPositionToHL( m_targetPosition, hlPosition ); + if ( !m_handler->ShouldMoveTo( m_pObject, hlPosition ) ) + return 0; + } + + IVP_Real_Object *pivp = m_pObject->GetObject(); + IVP_U_Quat targetOrientation; + IVP_U_Point outPosition; + + pivp->get_quat_world_f_object_AT( &targetOrientation, &outPosition ); + + if ( pivp->is_collision_detection_enabled() ) + { + m_pObject->EnableCollisions( false ); + pivp->beam_object_to_new_position( &targetOrientation, &m_targetPosition, IVP_TRUE ); + m_pObject->EnableCollisions( true ); + } + else + { + pivp->beam_object_to_new_position( &targetOrientation, &m_targetPosition, IVP_TRUE ); + } + m_forceTeleport = false; + return 1; +} + +void CPlayerController::StepUp( float height ) +{ + if ( height == 0.0f ) + return; + + Vector step( 0, 0, height ); + + IVP_Real_Object *pIVP = m_pObject->GetObject(); + IVP_U_Quat world_f_object; + IVP_U_Point positionIVP, deltaIVP; + ConvertPositionToIVP( step, deltaIVP ); + pIVP->get_quat_world_f_object_AT( &world_f_object, &positionIVP ); + positionIVP.add( &deltaIVP ); + pIVP->beam_object_to_new_position( &world_f_object, &positionIVP, IVP_TRUE ); +} + +void CPlayerController::Jump() +{ +#if 0 + // float for one tick to allow stepping and jumping to work properly + IVP_Real_Object *pIVP = m_pObject->GetObject(); + const IVP_U_Point *pgrav = pIVP->get_environment()->get_gravity(); + IVP_U_Float_Point gravSpeed; + gravSpeed.set_multiple( pgrav, pIVP->get_environment()->get_delta_PSI_time() ); + pIVP->get_core()->speed.subtract( &gravSpeed ); +#endif +} + +const int MAX_LIST_NORMALS = 8; +class CNormalList +{ +public: + bool IsFull() { return m_Normals.Count() == MAX_LIST_NORMALS; } + void AddNormal( const Vector &normal ) + { + if ( IsFull() ) + return; + + for ( int i = m_Normals.Count(); --i >= 0; ) + { + if ( DotProduct( m_Normals[i], normal ) > 0.99f ) + return; + } + m_Normals.AddToTail( normal ); + } + + bool HasPositiveProjection( const Vector &vec ) + { + for ( int i = m_Normals.Count(); --i >= 0; ) + { + if ( DotProduct( m_Normals[i], vec ) > 0 ) + return true; + } + return false; + } + + // UNDONE: Handle the case better where we clamp to multiple planes + // and still have a projection, but don't exceed limitVel. Currently that will stop. + // when this is done, remove the ground exception below. + Vector ClampVector( const Vector &inVector, float limitVel ) + { + if ( m_Normals.Count() > 2 ) + { + for ( int i = 0; i < m_Normals.Count(); i++ ) + { + if ( DotProduct(inVector, m_Normals[i]) > 0 ) + { + return vec3_origin; + } + } + } + else + { + if ( m_Normals.Count() == 2 ) + { + Vector crease; + CrossProduct( m_Normals[0], m_Normals[1], crease ); + float dot = DotProduct( inVector, crease ); + return crease * dot; + } + else if (m_Normals.Count() == 1) + { + float dot = DotProduct( inVector, m_Normals[0] ); + if ( dot > limitVel ) + { + return inVector + m_Normals[0]*(limitVel - dot); + } + } + } + return inVector; + } +private: + CUtlVectorFixed<Vector, MAX_LIST_NORMALS> m_Normals; +}; + +void CPlayerController::do_simulation_controller( IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> *) +{ + if ( !m_enable ) + return; + IVP_Real_Object *pivp = m_pObject->GetObject(); + + IVP_Core *pCore = pivp->get_core(); + Assert(!pCore->pinned && !pCore->physical_unmoveable); + // current situation + const IVP_U_Matrix *m_world_f_core = pCore->get_m_world_f_core_PSI(); + const IVP_U_Point *cur_pos_ws = m_world_f_core->get_position(); + + IVP_U_Float_Point baseVelocity; + baseVelocity.set_to_zero(); + // --------------------------------------------------------- + // Translation + // --------------------------------------------------------- + + if ( m_pGround ) + { + const IVP_U_Matrix *pMatrix = m_pGround->GetObject()->get_core()->get_m_world_f_core_PSI(); + pMatrix->vmult4( &m_groundPosition, &m_targetPosition ); + m_pGround->GetObject()->get_core()->get_surface_speed( &m_groundPosition, &baseVelocity ); + pCore->speed.subtract( &baseVelocity ); + } + + IVP_U_Float_Point delta_position; delta_position.subtract( &m_targetPosition, cur_pos_ws); + + if (!pivp->flags.shift_core_f_object_is_zero) + { + IVP_U_Float_Point shift_cs_os_ws; + m_world_f_core->vmult3( pivp->get_shift_core_f_object(), &shift_cs_os_ws); + delta_position.subtract( &shift_cs_os_ws ); + } + + + IVP_DOUBLE qdist = delta_position.quad_length(); + + // UNDONE: This is totally bogus! Measure error using last known estimate + // not current position! + if ( m_forceTeleport || qdist > m_maxDeltaPosition * m_maxDeltaPosition ) + { + if ( TryTeleportObject() ) + return; + } + + // float to allow stepping + const IVP_U_Point *pgrav = es->environment->get_gravity(); + IVP_U_Float_Point gravSpeed; + gravSpeed.set_multiple( pgrav, es->delta_time ); + if ( m_onground ) + { + pCore->speed.subtract( &gravSpeed ); + } + + float fraction = 1.0; + if ( m_secondsToArrival > 0 ) + { + fraction = es->delta_time / m_secondsToArrival; + if ( fraction > 1 ) + { + fraction = 1; + } + } + if ( !m_updatedSinceLast ) + { + // we haven't received an update from the game code since the last controller step + // This means we haven't gotten feedback integrated into the motion plan, so the error may be + // exaggerated. Assume that the first updated tick had valid information, and limit + // all subsequent ticks to the same size impulses. + // NOTE: Don't update the saved impulse - so any subsequent ticks will still have the last + // known good information. + float len = m_lastImpulse.real_length(); + // cap the max speed to the length of the last known good impulse + IVP_U_Float_Point tmp; + tmp.set( len, len, len ); + ComputeController( pCore->speed, delta_position, tmp, fraction / es->delta_time, m_dampFactor, NULL ); + } + else + { + ComputeController( pCore->speed, delta_position, m_maxSpeed, fraction / es->delta_time, m_dampFactor, &m_lastImpulse ); + } + pCore->speed.add( &baseVelocity ); + m_updatedSinceLast = false; + + // UNDONE: Assumes gravity points down + Vector lastImpulseHL; + ConvertPositionToHL( pCore->speed, lastImpulseHL ); + IPhysicsFrictionSnapshot *pSnapshot = CreateFrictionSnapshot( pivp ); + bool bGround = false; + float invMass = pivp->get_core()->get_inv_mass(); + float limitVel = m_pushableSpeedLimit; + CNormalList normalList; + while (pSnapshot->IsValid()) + { + Vector normal; + pSnapshot->GetSurfaceNormal( normal ); + if ( normal.z < -0.7f ) + { + bGround = true; + } + // remove this when clamp works better + if ( normal.z > -0.99f ) + { + IPhysicsObject *pOther = pSnapshot->GetObject(1); + if ( !pOther->IsMoveable() || pOther->GetMass() > m_pushableMassLimit ) + { + limitVel = 0.0f; + } + float pushSpeed = DotProduct( lastImpulseHL, normal ); + float contactVel = pSnapshot->GetNormalForce() * invMass; + float pushTotal = pushSpeed + contactVel; + if ( pushTotal > limitVel ) + { + normalList.AddNormal( normal ); + } + } + + pSnapshot->NextFrictionData(); + } + DestroyFrictionSnapshot( pSnapshot ); + + Vector clamped = normalList.ClampVector( lastImpulseHL, limitVel ); + Vector limit = clamped - lastImpulseHL; + IVP_U_Float_Point limitIVP; + ConvertPositionToIVP( limit, limitIVP ); + pivp->get_core()->speed.add( &limitIVP ); + m_lastImpulse.add( &limitIVP ); + + if ( bGround ) + { + float gravDt = gravSpeed.real_length(); + // moving down? Press down with full gravity and no more + if ( m_lastImpulse.k[1] >= 0 ) + { + float delta = gravDt - m_lastImpulse.k[1]; + pivp->get_core()->speed.k[1] += delta; + m_lastImpulse.k[1] += delta; + } + } + + // if we have time left, subtract it off + m_secondsToArrival -= es->delta_time; + if ( m_secondsToArrival < 0 ) + { + m_secondsToArrival = 0; + } +} + +void CPlayerController::SetEventHandler( IPhysicsPlayerControllerEvent *handler ) +{ + m_handler = handler; +} + +void CPlayerController::Update( const Vector& position, const Vector& velocity, float secondsToArrival, bool onground, IPhysicsObject *ground ) +{ + IVP_U_Point targetPositionIVP; + IVP_U_Float_Point targetSpeedIVP; + + ConvertPositionToIVP( position, targetPositionIVP ); + ConvertPositionToIVP( velocity, targetSpeedIVP ); + + m_updatedSinceLast = true; + // if the object hasn't moved, abort + if ( targetSpeedIVP.quad_distance_to( &m_currentSpeed ) < 1e-6 ) + { + if ( targetPositionIVP.quad_distance_to( &m_targetPosition ) < 1e-6 ) + { + return; + } + } + + m_targetPosition.set( &targetPositionIVP ); + m_secondsToArrival = secondsToArrival < 0 ? 0 : secondsToArrival; + // Sanity check to make sure the position is good. +#ifdef _DEBUG + float large = 1024 * 512; + Assert( m_targetPosition.k[0] >= -large && m_targetPosition.k[0] <= large ); + Assert( m_targetPosition.k[1] >= -large && m_targetPosition.k[1] <= large ); + Assert( m_targetPosition.k[2] >= -large && m_targetPosition.k[2] <= large ); +#endif + + m_currentSpeed.set( &targetSpeedIVP ); + + IVP_Real_Object *pivp = m_pObject->GetObject(); + IVP_Core *pCore = pivp->get_core(); + IVP_Environment *pEnv = pivp->get_environment(); + pEnv->get_controller_manager()->ensure_core_in_simulation(pCore); + + m_enable = true; + // m_onground makes this object anti-grav + // UNDONE: Re-evaluate this + m_onground = false;//onground; + if ( velocity.LengthSqr() <= 0.1f ) + { + // no input velocity, just go where physics takes you. + m_enable = false; + ground = NULL; + } + else + { + MaxSpeed( velocity ); + } + + CPhysicsObject *pGroundObject = static_cast<CPhysicsObject *>(ground); + SetGround( pGroundObject ); + if ( m_pGround ) + { + const IVP_U_Matrix *pMatrix = m_pGround->GetObject()->get_core()->get_m_world_f_core_PSI(); + pMatrix->vimult4( &m_targetPosition, &m_groundPosition ); + } +} + +void CPlayerController::MaxSpeed( const Vector &velocity ) +{ + IVP_Core *pCore = m_pObject->GetObject()->get_core(); + IVP_U_Float_Point ivpVel; + ConvertPositionToIVP( velocity, ivpVel ); + IVP_U_Float_Point available = ivpVel; + + // normalize and save length + float length = ivpVel.real_length_plus_normize(); + + IVP_U_Float_Point baseVelocity; + baseVelocity.set_to_zero(); + + float dot = ivpVel.dot_product( &pCore->speed ); + if ( dot > 0 ) + { + ivpVel.mult( dot * length ); + available.subtract( &ivpVel ); + } + + pCore->speed.add( &baseVelocity ); + IVP_Float_PointAbs( m_maxSpeed, available ); +} + + +void CPlayerController::GetShadowVelocity( Vector *velocity ) +{ + IVP_Core *core = m_pObject->GetObject()->get_core(); + if ( velocity ) + { + IVP_U_Float_Point speed; + speed.add( &core->speed, &core->speed_change ); + if ( m_pGround ) + { + IVP_U_Float_Point baseVelocity; + m_pGround->GetObject()->get_core()->get_surface_speed( &m_groundPosition, &baseVelocity ); + speed.subtract( &baseVelocity ); + } + ConvertPositionToHL( speed, *velocity ); + } +} + + +bool CPlayerController::IsInContact( void ) +{ + IVP_Real_Object *pivp = m_pObject->GetObject(); + if ( !pivp->flags.collision_detection_enabled ) + return false; + + IVP_Synapse_Friction *pfriction = pivp->get_first_friction_synapse(); + while ( pfriction ) + { + extern IVP_Real_Object *GetOppositeSynapseObject( IVP_Synapse_Friction *pfriction ); + + IVP_Real_Object *pobj = GetOppositeSynapseObject( pfriction ); + if ( pobj->flags.collision_detection_enabled ) + { + // skip if this is a static object + if ( !pobj->get_core()->physical_unmoveable && !pobj->get_core()->pinned ) + { + CPhysicsObject *pPhys = static_cast<CPhysicsObject *>(pobj->client_data); + // If this is a game-controlled shadow object, then skip it. + // otherwise, we're in contact with something physically simulated + if ( !pPhys->IsControlledByGame() ) + return true; + } + } + + pfriction = pfriction->get_next(); + } + + return false; +} + + +IPhysicsPlayerController *CreatePlayerController( CPhysicsObject *pObject ) +{ + return new CPlayerController( pObject ); +} + +void DestroyPlayerController( IPhysicsPlayerController *pController ) +{ + delete pController; +} + +void QuaternionDiff( const IVP_U_Quat &p, const IVP_U_Quat &q, IVP_U_Quat &qt ) +{ + IVP_U_Quat q2; + + // decide if one of the quaternions is backwards + q2.set_invert_unit_quat( &q ); + qt.set_mult_quat( &q2, &p ); + qt.normize_quat(); +} + + +void QuaternionAxisAngle( const IVP_U_Quat &q, Vector &axis, float &angle ) +{ + angle = 2 * acos(q.w); + if ( angle > M_PI ) + { + angle -= 2*M_PI; + } + + axis.Init( q.x, q.y, q.z ); + VectorNormalize( axis ); +} + + +void GetObjectPosition_IVP( IVP_U_Point &origin, IVP_Real_Object *pivp ) +{ + const IVP_U_Matrix *m_world_f_core = pivp->get_core()->get_m_world_f_core_PSI(); + origin.set( m_world_f_core->get_position() ); + if (!pivp->flags.shift_core_f_object_is_zero) + { + IVP_U_Float_Point shift_cs_os_ws; + m_world_f_core->vmult3( pivp->get_shift_core_f_object(), &shift_cs_os_ws ); + origin.add(&shift_cs_os_ws); + } +} + + +bool IsZeroVector( const IVP_U_Point &vec ) +{ + return (vec.k[0] == 0.0 && vec.k[1] == 0.0 && vec.k[2] == 0.0 ) ? true : false; +} + +float ComputeShadowControllerIVP( IVP_Real_Object *pivp, shadowcontrol_params_t ¶ms, float secondsToArrival, float dt ) +{ + // resample fraction + // This allows us to arrive at the target at the requested time + float fraction = 1.0; + if ( secondsToArrival > 0 ) + { + fraction = dt / secondsToArrival; + if ( fraction > 1 ) + { + fraction = 1; + } + } + + secondsToArrival -= dt; + if ( secondsToArrival < 0 ) + { + secondsToArrival = 0; + } + + if ( fraction <= 0 ) + return secondsToArrival; + + // --------------------------------------------------------- + // Translation + // --------------------------------------------------------- + + IVP_U_Point positionIVP; + GetObjectPosition_IVP( positionIVP, pivp ); + + IVP_U_Float_Point delta_position; + delta_position.subtract( ¶ms.targetPosition, &positionIVP); + + // BUGBUG: Save off velocities and estimate final positions + // measure error against these final sets + // also, damp out 100% saved velocity, use max additional impulses + // to correct error and damp out error velocity + // extrapolate position + if ( params.teleportDistance > 0 ) + { + IVP_DOUBLE qdist; + if ( !IsZeroVector(params.lastPosition) ) + { + IVP_U_Float_Point tmpDelta; + tmpDelta.subtract( &positionIVP, ¶ms.lastPosition ); + qdist = tmpDelta.quad_length(); + } + else + { + // UNDONE: This is totally bogus! Measure error using last known estimate + // not current position! + qdist = delta_position.quad_length(); + } + + if ( qdist > params.teleportDistance * params.teleportDistance ) + { + if ( pivp->is_collision_detection_enabled() ) + { + pivp->enable_collision_detection( IVP_FALSE ); + pivp->beam_object_to_new_position( ¶ms.targetRotation, ¶ms.targetPosition, IVP_TRUE ); + pivp->enable_collision_detection( IVP_TRUE ); + } + else + { + pivp->beam_object_to_new_position( ¶ms.targetRotation, ¶ms.targetPosition, IVP_TRUE ); + } + delta_position.set_to_zero(); + } + } + + float invDt = 1.0f / dt; + IVP_Core *pCore = pivp->get_core(); + ComputeController( pCore->speed, delta_position, params.maxSpeed, params.maxDampSpeed, fraction * invDt, params.dampFactor, ¶ms.lastImpulse ); + + params.lastPosition.add_multiple( &positionIVP, &pCore->speed, dt ); + + IVP_U_Float_Point deltaAngles; + // compute rotation offset + IVP_U_Quat deltaRotation; + QuaternionDiff( params.targetRotation, pCore->q_world_f_core_next_psi, deltaRotation ); + + // convert offset to angular impulse + Vector axis; + float angle; + QuaternionAxisAngle( deltaRotation, axis, angle ); + VectorNormalize(axis); + + deltaAngles.k[0] = axis.x * angle; + deltaAngles.k[1] = axis.y * angle; + deltaAngles.k[2] = axis.z * angle; + + ComputeController( pCore->rot_speed, deltaAngles, params.maxAngular, params.maxDampAngular, fraction * invDt, params.dampFactor, NULL ); + + return secondsToArrival; +} + +void ConvertShadowControllerToIVP( const hlshadowcontrol_params_t &in, shadowcontrol_params_t &out ) +{ + ConvertPositionToIVP( in.targetPosition, out.targetPosition ); + ConvertRotationToIVP( in.targetRotation, out.targetRotation ); + out.teleportDistance = ConvertDistanceToIVP( in.teleportDistance ); + + out.maxSpeed = ConvertDistanceToIVP( in.maxSpeed ); + out.maxDampSpeed = ConvertDistanceToIVP( in.maxDampSpeed ); + out.maxAngular = ConvertAngleToIVP( in.maxAngular ); + out.maxDampAngular = ConvertAngleToIVP( in.maxDampAngular ); + + out.dampFactor = in.dampFactor; +} + +void ConvertShadowControllerToHL( const shadowcontrol_params_t &in, hlshadowcontrol_params_t &out ) +{ + ConvertPositionToHL( in.targetPosition, out.targetPosition ); + ConvertRotationToHL( in.targetRotation, out.targetRotation ); + out.teleportDistance = ConvertDistanceToHL( in.teleportDistance ); + + out.maxSpeed = ConvertDistanceToHL( in.maxSpeed ); + out.maxDampSpeed = ConvertDistanceToHL( in.maxDampSpeed ); + out.maxAngular = ConvertAngleToHL( in.maxAngular ); + out.maxDampAngular = ConvertAngleToHL( in.maxDampAngular ); + + out.dampFactor = in.dampFactor; +} + +float ComputeShadowControllerHL( CPhysicsObject *pObject, const hlshadowcontrol_params_t ¶ms, float secondsToArrival, float dt ) +{ + shadowcontrol_params_t ivpParams; + ConvertShadowControllerToIVP( params, ivpParams ); + return ComputeShadowControllerIVP( pObject->GetObject(), ivpParams, secondsToArrival, dt ); +} + +class CShadowController : public IVP_Controller_Independent, public IPhysicsShadowController, public CAlignedNewDelete<16> +{ +public: + CShadowController(); + CShadowController( CPhysicsObject *pObject, bool allowTranslation, bool allowRotation ); + ~CShadowController( void ); + + // ipion interfaces + void do_simulation_controller( IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> *cores); + virtual IVP_CONTROLLER_PRIORITY get_controller_priority() { return IVP_CP_MOTION; } + virtual const char *get_controller_name() { return "vphysics:shadow"; } + + void SetObject( IPhysicsObject *pObject ); + void Update( const Vector &position, const QAngle &angles, float secondsToArrival ); + void MaxSpeed( float maxSpeed, float maxAngularSpeed ); + virtual void StepUp( float height ); + virtual void SetTeleportDistance( float teleportDistance ); + virtual bool AllowsTranslation() { return m_allowsTranslation; } + virtual bool AllowsRotation() { return m_allowsRotation; } + + virtual void GetLastImpulse( Vector *pOut ) + { + ConvertPositionToHL( m_shadow.lastImpulse, *pOut ); + } + bool IsEnabled() { return m_enabled; } + void Enable( bool bEnable ) + { + m_enabled = bEnable; + } + + void WriteToTemplate( vphysics_save_cshadowcontroller_t &controllerTemplate ); + void InitFromTemplate( const vphysics_save_cshadowcontroller_t &controllerTemplate ); + + virtual void SetPhysicallyControlled( bool isPhysicallyControlled ) { m_isPhysicallyControlled = isPhysicallyControlled; } + virtual bool IsPhysicallyControlled() { return m_isPhysicallyControlled; } + + virtual void UseShadowMaterial( bool bUseShadowMaterial ) + { + if ( !m_pObject ) + return; + + int current = m_pObject->GetMaterialIndexInternal(); + int target = bUseShadowMaterial ? MATERIAL_INDEX_SHADOW : m_savedMaterialIndex; + if ( target != current ) + { + m_pObject->SetMaterialIndex( target ); + } + } + + virtual void ObjectMaterialChanged( int materialIndex ) + { + if ( !m_pObject ) + return; + m_savedMaterialIndex = materialIndex; + } + + //Basically get the last inputs to IPhysicsShadowController::Update(), returns last input to timeOffset in Update() + virtual float GetTargetPosition( Vector *pPositionOut, QAngle *pAnglesOut ); + + virtual float GetTeleportDistance( void ); + virtual void GetMaxSpeed( float *pMaxSpeedOut, float *pMaxAngularSpeedOut ); + +private: + void AttachObject( void ); + void DetachObject( void ); + + shadowcontrol_params_t m_shadow; + IVP_U_Float_Point m_saveRot; + IVP_U_Float_Point m_savedRI; + CPhysicsObject *m_pObject; + float m_secondsToArrival; + float m_savedMass; + unsigned int m_savedFlags; + + unsigned short m_savedMaterialIndex; + bool m_enabled : 1; + bool m_allowsTranslation : 1; + bool m_allowsRotation : 1; + bool m_isPhysicallyControlled : 1; +}; + +CShadowController::CShadowController() +{ + m_shadow.targetPosition.set_to_zero(); + m_shadow.targetRotation.init(); +} + +CShadowController::CShadowController( CPhysicsObject *pObject, bool allowTranslation, bool allowRotation ) +{ + m_pObject = pObject; + m_shadow.dampFactor = 1.0f; + m_shadow.teleportDistance = 0; + m_shadow.maxDampSpeed = 0; + m_shadow.maxDampAngular = 0; + m_shadow.targetPosition.set_to_zero(); + m_shadow.targetRotation.init(); + + m_allowsTranslation = allowTranslation; + m_allowsRotation = allowRotation; + + m_isPhysicallyControlled = false; + m_enabled = false; + + AttachObject(); +} + +CShadowController::~CShadowController( void ) +{ + DetachObject(); +} + +void CShadowController::AttachObject( void ) +{ + IVP_Real_Object *pivp = m_pObject->GetObject(); + IVP_Core *pCore = pivp->get_core(); + m_saveRot = pCore->rot_speed_damp_factor; + m_savedRI = *pCore->get_rot_inertia(); + m_savedMass = pCore->get_mass(); + m_savedMaterialIndex = m_pObject->GetMaterialIndexInternal(); + + Vector position; + QAngle angles; + m_pObject->GetPosition( &position, &angles ); + ConvertPositionToIVP( position, m_shadow.targetPosition ); + ConvertRotationToIVP( angles, m_shadow.targetRotation ); + + UseShadowMaterial( true ); + + pCore->rot_speed_damp_factor = IVP_U_Float_Point( 100, 100, 100 ); + + if ( !m_allowsRotation ) + { + IVP_U_Float_Point ri( 1e15f, 1e15f, 1e15f ); + pCore->set_rotation_inertia( &ri ); + } + if ( !m_allowsTranslation ) + { + m_pObject->SetMass( VPHYSICS_MAX_MASS ); + //pCore->inv_rot_inertia.hesse_val = 0.0f; + m_pObject->EnableGravity( false ); + } + + m_savedFlags = m_pObject->CallbackFlags(); + unsigned int flags = m_savedFlags | CALLBACK_SHADOW_COLLISION; + flags &= ~CALLBACK_GLOBAL_FRICTION; + flags &= ~CALLBACK_GLOBAL_COLLIDE_STATIC; + m_pObject->SetCallbackFlags( flags ); + m_pObject->EnableDrag( false ); + + pCore->calc_calc(); + pivp->get_environment()->get_controller_manager()->add_controller_to_core( this, pCore ); + m_shadow.lastPosition.set_to_zero(); +} + +void CShadowController::DetachObject( void ) +{ + IVP_Real_Object *pivp = m_pObject->GetObject(); + IVP_Core *pCore = pivp->get_core(); + // don't bother if we're just doing this to delete the object + if ( !(m_pObject->GetCallbackFlags() & CALLBACK_MARKED_FOR_DELETE) ) + { + pCore->rot_speed_damp_factor = m_saveRot; + pCore->set_mass( m_savedMass ); + m_pObject->SetCallbackFlags( m_savedFlags ); + m_pObject->EnableDrag( true ); + m_pObject->EnableGravity( true ); + UseShadowMaterial( false ); + pCore->set_rotation_inertia( &m_savedRI ); // this calls calc_calc() + } + m_pObject = NULL; + pivp->get_environment()->get_controller_manager()->remove_controller_from_core( this, pCore ); +} + +void CShadowController::SetObject( IPhysicsObject *pObject ) +{ + CPhysicsObject *obj = (CPhysicsObject *)pObject; + if ( obj == m_pObject ) + return; + + DetachObject(); + m_pObject = obj; + AttachObject(); +} + + +void CShadowController::StepUp( float height ) +{ + Vector step( 0, 0, height ); + + IVP_Real_Object *pIVP = m_pObject->GetObject(); + IVP_U_Quat world_f_object; + IVP_U_Point positionIVP, deltaIVP; + ConvertPositionToIVP( step, deltaIVP ); + pIVP->get_quat_world_f_object_AT( &world_f_object, &positionIVP ); + positionIVP.add( &deltaIVP ); + pIVP->beam_object_to_new_position( &world_f_object, &positionIVP, IVP_TRUE ); +} + +void CShadowController::SetTeleportDistance( float teleportDistance ) +{ + m_shadow.teleportDistance = ConvertDistanceToIVP( teleportDistance ); +} + +float CShadowController::GetTeleportDistance( void ) +{ + return ConvertDistanceToHL( m_shadow.teleportDistance ); +} + +void CShadowController::do_simulation_controller( IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> *) +{ + if ( IsEnabled() ) + { + IVP_Real_Object *pivp = m_pObject->GetObject(); + Assert(!pivp->get_core()->pinned && !pivp->get_core()->physical_unmoveable); + + ComputeShadowControllerIVP( pivp, m_shadow, m_secondsToArrival, es->delta_time ); + if ( m_allowsTranslation ) + { + // UNDONE: Assumes gravity points down + const IVP_U_Point *pgrav = pivp->get_environment()->get_gravity(); + float gravDt = pgrav->k[1] * es->delta_time; + + if ( m_shadow.lastImpulse.k[1] > gravDt ) + { + if ( IsOnGround( pivp ) ) + { + float delta = gravDt - m_shadow.lastImpulse.k[1]; + pivp->get_core()->speed.k[1] += delta; + m_shadow.lastImpulse.k[1] += delta; + } + } + } + + // if we have time left, subtract it off + m_secondsToArrival -= es->delta_time; + if ( m_secondsToArrival < 0 ) + { + m_secondsToArrival = 0; + } + } + else + { + m_shadow.lastPosition.set_to_zero(); + } +} + +// NOTE: This isn't a test for equivalent orientations, it's a test for calling update +// with EXACTLY the same data repeatedly +static bool IsEqual( const IVP_U_Point &pt0, const IVP_U_Point &pt1 ) +{ + return pt0.quad_distance_to( &pt1 ) < 1e-8f ? true : false; +} + +// NOTE: This isn't a test for equivalent orientations, it's a test for calling update +// with EXACTLY the same data repeatedly +static bool IsEqual( const IVP_U_Quat &pt0, const IVP_U_Quat &pt1 ) +{ + float delta = fabs(pt0.x - pt1.x); + delta += fabs(pt0.y - pt1.y); + delta += fabs(pt0.z - pt1.z); + delta += fabs(pt0.w - pt1.w); + return delta < 1e-8f ? true : false; +} + +void CShadowController::Update( const Vector &position, const QAngle &angles, float secondsToArrival ) +{ + IVP_U_Point targetPosition = m_shadow.targetPosition; + IVP_U_Quat targetRotation = m_shadow.targetRotation; + + ConvertPositionToIVP( position, m_shadow.targetPosition ); + m_secondsToArrival = secondsToArrival < 0 ? 0 : secondsToArrival; + ConvertRotationToIVP( angles, m_shadow.targetRotation ); + + Enable( true ); + + if ( IsEqual( targetPosition, m_shadow.targetPosition ) && IsEqual( targetRotation, m_shadow.targetRotation ) ) + return; + + m_pObject->Wake(); +} + +float CShadowController::GetTargetPosition( Vector *pPositionOut, QAngle *pAnglesOut ) +{ + if( pPositionOut ) + ConvertPositionToHL( m_shadow.targetPosition, *pPositionOut ); + + if( pAnglesOut ) + ConvertRotationToHL( m_shadow.targetRotation, *pAnglesOut ); + + return m_secondsToArrival; +} + +void CShadowController::MaxSpeed( float maxSpeed, float maxAngularSpeed ) +{ + // UNDONE: Turn this on when shadow controllers are having velocity updated per frame + // right now this has the effect of making dampspeed zero by default. +#if 0 + IVP_Core *pCore = m_pObject->GetObject()->get_core(); + { + // limit additional velocity to that which is not amplifying the current velocity + float availableSpeed = ConvertDistanceToIVP( maxSpeed ); + float currentSpeed = pCore->speed.real_length(); + + m_shadow.maxDampSpeed = min(currentSpeed, availableSpeed); + m_shadow.maxSpeed = availableSpeed - m_shadow.maxDampSpeed; + } + + { + // limit additional velocity to that which is not amplifying the current velocity + float availableAngularSpeed = ConvertAngleToIVP( maxAngularSpeed ); + float currentAngularSpeed = pCore->rot_speed.real_length(); + m_shadow.maxDampAngular = min(currentAngularSpeed, availableAngularSpeed); + m_shadow.maxAngular = availableAngularSpeed - m_shadow.maxDampAngular; + } +#else + m_shadow.maxSpeed = maxSpeed; + m_shadow.maxDampSpeed = maxSpeed; + m_shadow.maxAngular = maxAngularSpeed; + m_shadow.maxDampAngular = maxAngularSpeed; +#endif +} + +void CShadowController::GetMaxSpeed( float *pMaxSpeedOut, float *pMaxAngularSpeedOut ) +{ + if( pMaxSpeedOut ) + *pMaxSpeedOut = m_shadow.maxSpeed; + + if( pMaxAngularSpeedOut ) + *pMaxAngularSpeedOut = m_shadow.maxAngular; +} + +struct vphysics_save_shadowcontrolparams_t : public hlshadowcontrol_params_t +{ + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_shadowcontrolparams_t ) + DEFINE_FIELD( targetPosition, FIELD_POSITION_VECTOR ), + DEFINE_FIELD( targetRotation, FIELD_VECTOR ), + DEFINE_FIELD( maxSpeed, FIELD_FLOAT ), + DEFINE_FIELD( maxDampSpeed, FIELD_FLOAT ), + DEFINE_FIELD( maxAngular, FIELD_FLOAT ), + DEFINE_FIELD( maxDampAngular, FIELD_FLOAT ), + DEFINE_FIELD( dampFactor, FIELD_FLOAT ), + DEFINE_FIELD( teleportDistance, FIELD_FLOAT ), +END_DATADESC() + +struct vphysics_save_cshadowcontroller_t +{ + CPhysicsObject *pObject; + float secondsToArrival; + IVP_U_Float_Point saveRot; + IVP_U_Float_Point savedRI; + IVP_U_Float_Point currentSpeed; + + float savedMass; + int savedMaterial; + unsigned int savedFlags; + bool enable; + bool allowPhysicsMovement; + bool allowPhysicsRotation; + bool isPhysicallyControlled; + + hlshadowcontrol_params_t shadowParams; + + DECLARE_SIMPLE_DATADESC(); +}; + + +BEGIN_SIMPLE_DATADESC( vphysics_save_cshadowcontroller_t ) + //DEFINE_VPHYSPTR( pObject ), + DEFINE_FIELD( secondsToArrival, FIELD_FLOAT ), + DEFINE_ARRAY( saveRot.k, FIELD_FLOAT, 3 ), + DEFINE_ARRAY( savedRI.k, FIELD_FLOAT, 3 ), + DEFINE_ARRAY( currentSpeed.k, FIELD_FLOAT, 3 ), + DEFINE_FIELD( savedMass, FIELD_FLOAT ), + DEFINE_FIELD( savedFlags, FIELD_INTEGER ), + DEFINE_CUSTOM_FIELD( savedMaterial, MaterialIndexDataOps() ), + DEFINE_FIELD( enable, FIELD_BOOLEAN ), + DEFINE_FIELD( allowPhysicsMovement, FIELD_BOOLEAN ), + DEFINE_FIELD( allowPhysicsRotation, FIELD_BOOLEAN ), + DEFINE_FIELD( isPhysicallyControlled, FIELD_BOOLEAN ), + + DEFINE_EMBEDDED_OVERRIDE( shadowParams, vphysics_save_shadowcontrolparams_t ), +END_DATADESC() + +void CShadowController::WriteToTemplate( vphysics_save_cshadowcontroller_t &controllerTemplate ) +{ + controllerTemplate.pObject = m_pObject; + controllerTemplate.secondsToArrival = m_secondsToArrival; + controllerTemplate.saveRot = m_saveRot; + controllerTemplate.savedRI = m_savedRI; + controllerTemplate.savedMass = m_savedMass; + controllerTemplate.savedFlags = m_savedFlags; + + controllerTemplate.savedMaterial = m_savedMaterialIndex; + controllerTemplate.enable = IsEnabled(); + controllerTemplate.allowPhysicsMovement = m_allowsTranslation; + controllerTemplate.allowPhysicsRotation = m_allowsRotation; + controllerTemplate.isPhysicallyControlled = m_isPhysicallyControlled; + + ConvertShadowControllerToHL( m_shadow, controllerTemplate.shadowParams ); +} + +void CShadowController::InitFromTemplate( const vphysics_save_cshadowcontroller_t &controllerTemplate ) +{ + m_pObject = controllerTemplate.pObject; + m_secondsToArrival = controllerTemplate.secondsToArrival; + m_saveRot = controllerTemplate.saveRot; + m_savedRI = controllerTemplate.savedRI; + m_savedMass = controllerTemplate.savedMass; + m_savedFlags = controllerTemplate.savedFlags; + m_savedMaterialIndex = controllerTemplate.savedMaterial; + + Enable( controllerTemplate.enable ); + m_allowsTranslation = controllerTemplate.allowPhysicsMovement; + m_allowsRotation = controllerTemplate.allowPhysicsRotation; + m_isPhysicallyControlled = controllerTemplate.isPhysicallyControlled; + + ConvertShadowControllerToIVP( controllerTemplate.shadowParams, m_shadow ); + m_pObject->GetObject()->get_environment()->get_controller_manager()->add_controller_to_core( this, m_pObject->GetObject()->get_core() ); +} + + +IPhysicsShadowController *CreateShadowController( CPhysicsObject *pObject, bool allowTranslation, bool allowRotation ) +{ + return new CShadowController( pObject, allowTranslation, allowRotation ); +} + + + +bool SavePhysicsShadowController( const physsaveparams_t ¶ms, IPhysicsShadowController *pIShadow ) +{ + vphysics_save_cshadowcontroller_t controllerTemplate; + memset( &controllerTemplate, 0, sizeof(controllerTemplate) ); + + CShadowController *pShadowController = (CShadowController *)pIShadow; + pShadowController->WriteToTemplate( controllerTemplate ); + params.pSave->WriteAll( &controllerTemplate ); + return true; +} + +bool RestorePhysicsShadowController( const physrestoreparams_t ¶ms, IPhysicsShadowController **ppShadowController ) +{ + return false; +} + +bool RestorePhysicsShadowControllerInternal( const physrestoreparams_t ¶ms, IPhysicsShadowController **ppShadowController, CPhysicsObject *pObject ) +{ + vphysics_save_cshadowcontroller_t controllerTemplate; + + memset( &controllerTemplate, 0, sizeof(controllerTemplate) ); + params.pRestore->ReadAll( &controllerTemplate ); + + // HACKHACK: pass this in + controllerTemplate.pObject = pObject; + + CShadowController *pShadow = new CShadowController(); + pShadow->InitFromTemplate( controllerTemplate ); + + *ppShadowController = pShadow; + return true; +} + +bool SavePhysicsPlayerController( const physsaveparams_t ¶ms, CPlayerController *pPlayerController ) +{ + return false; +} + +bool RestorePhysicsPlayerController( const physrestoreparams_t ¶ms, CPlayerController **ppPlayerController ) +{ + return false; +} + +//HACKHACK: The physics object transfer system needs to temporarily detach a shadow controller from an object, then reattach without other repercussions +void ControlPhysicsShadowControllerAttachment_Silent( IPhysicsShadowController *pController, IVP_Real_Object *pivp, bool bAttach ) +{ + if( bAttach ) + { + pivp->get_environment()->get_controller_manager()->add_controller_to_core( (CShadowController *)pController, pivp->get_core() ); + } + else + { + pivp->get_environment()->get_controller_manager()->remove_controller_from_core( (CShadowController *)pController, pivp->get_core() ); + } +} + +void ControlPhysicsPlayerControllerAttachment_Silent( IPhysicsPlayerController *pController, IVP_Real_Object *pivp, bool bAttach ) +{ + if( bAttach ) + { + pivp->get_environment()->get_controller_manager()->add_controller_to_core( (CPlayerController *)pController, pivp->get_core() ); + } + else + { + pivp->get_environment()->get_controller_manager()->remove_controller_from_core( (CPlayerController *)pController, pivp->get_core() ); + } +} + diff --git a/vphysics/physics_shadow.h b/vphysics/physics_shadow.h new file mode 100644 index 0000000..766a33d --- /dev/null +++ b/vphysics/physics_shadow.h @@ -0,0 +1,49 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_SHADOW_H +#define PHYSICS_SHADOW_H +#pragma once + +class CPhysicsObject; +class IPhysicsShadowController; +class IPhysicsPlayerController; + +extern IPhysicsShadowController *CreateShadowController( CPhysicsObject *pObject, bool allowTranslation, bool allowRotation ); +extern IPhysicsPlayerController *CreatePlayerController( CPhysicsObject *pObject ); +extern void DestroyPlayerController( IPhysicsPlayerController *pController ); + + +extern void ComputeController( IVP_U_Float_Point ¤tSpeed, const IVP_U_Float_Point &delta, const IVP_U_Float_Point &maxSpeed, float scaleDelta, float damping ); + +#include "ivp_physics.hxx" + +struct shadowcontrol_params_t +{ + shadowcontrol_params_t() + { + lastPosition.set_to_zero(); + lastImpulse.set_to_zero(); + } + + IVP_U_Point targetPosition; + IVP_U_Quat targetRotation; + IVP_U_Point lastPosition; // estimate for where the object should be, used to compute error for teleport + IVP_U_Float_Point lastImpulse; // Impulse applied last frame + float maxAngular; + float maxDampAngular; + float maxSpeed; + float maxDampSpeed; + float dampFactor; + float teleportDistance; +}; + + +float ComputeShadowControllerHL( CPhysicsObject *pObject, const hlshadowcontrol_params_t ¶ms, float secondsToArrival, float dt ); +float ComputeShadowControllerIVP( IVP_Real_Object *pivp, shadowcontrol_params_t ¶ms, float secondsToArrival, float dt ); + +#endif // PHYSICS_SHADOW_H diff --git a/vphysics/physics_spring.cpp b/vphysics/physics_spring.cpp new file mode 100644 index 0000000..17f77a2 --- /dev/null +++ b/vphysics/physics_spring.cpp @@ -0,0 +1,286 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" + +#include "ivp_actuator.hxx" +#include "ivp_actuator_spring.hxx" +#include "ivp_listener_object.hxx" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +struct vphysics_save_cphysicsspring_t : public springparams_t +{ + CPhysicsObject *pObjStart; + CPhysicsObject *pObjEnd; + DECLARE_SIMPLE_DATADESC(); +}; + +BEGIN_SIMPLE_DATADESC( vphysics_save_cphysicsspring_t ) + DEFINE_FIELD( constant, FIELD_FLOAT ), + DEFINE_FIELD( naturalLength, FIELD_FLOAT ), + DEFINE_FIELD( damping, FIELD_FLOAT ), + DEFINE_FIELD( relativeDamping, FIELD_FLOAT ), + + // NOTE: These are stored as *relative* vectors, so we don't use FIELD_POSITION_VECTOR + DEFINE_FIELD( startPosition, FIELD_VECTOR ), + DEFINE_FIELD( endPosition, FIELD_VECTOR ), + DEFINE_FIELD( useLocalPositions, FIELD_BOOLEAN ), + DEFINE_FIELD( onlyStretch, FIELD_BOOLEAN ), + + DEFINE_VPHYSPTR( pObjStart ), + DEFINE_VPHYSPTR( pObjEnd ), +END_DATADESC() + + +// BUGBUG: No way to delete a spring because springs get auto-deleted without notification +// when an object they are attached to is deleted. +// So there is no way to tell if the pointer is valid. +class CPhysicsSpring : public IPhysicsSpring, public IVP_Listener_Object +{ +public: + CPhysicsSpring( CPhysicsObject *pObjectStart, CPhysicsObject *pObjectEnd, IVP_Actuator_Spring *pSpring ); + ~CPhysicsSpring(); + + // IPhysicsSpring + void GetEndpoints( Vector* worldPositionStart, Vector* worldPositionEnd ); + void SetSpringConstant( float flSpringContant); + void SetSpringDamping( float flSpringDamping); + void SetSpringLength( float flSpringLength); + IPhysicsObject *GetStartObject( void ) { return m_pObjStart; } + IPhysicsObject *GetEndObject( void ) { return m_pObjEnd; } + + void AttachListener(); + void DetachListener(); + + // Object listener + virtual void event_object_deleted( IVP_Event_Object *); + virtual void event_object_created( IVP_Event_Object *) {} + virtual void event_object_revived( IVP_Event_Object *) {} + virtual void event_object_frozen ( IVP_Event_Object *) {} + void WriteToTemplate( vphysics_save_cphysicsspring_t ¶ms ); + +private: + + CPhysicsSpring( void ); + IVP_Actuator_Spring *m_pSpring; + CPhysicsObject *m_pObjStart; + CPhysicsObject *m_pObjEnd; +}; + +CPhysicsSpring::CPhysicsSpring( CPhysicsObject *pObjectStart, CPhysicsObject *pObjectEnd, IVP_Actuator_Spring *pSpring ) : m_pSpring(pSpring) +{ + m_pObjStart = pObjectStart; + m_pObjEnd = pObjectEnd; + AttachListener(); +} + +void CPhysicsSpring::AttachListener() +{ + if ( !(m_pObjStart->CallbackFlags() & CALLBACK_NEVER_DELETED) ) + { + m_pObjStart->GetObject()->add_listener_object( this ); + } + + if ( !(m_pObjEnd->CallbackFlags() & CALLBACK_NEVER_DELETED) ) + { + m_pObjEnd->GetObject()->add_listener_object( this ); + } +} + +CPhysicsSpring::~CPhysicsSpring() +{ + if ( m_pSpring ) + { + delete m_pSpring; + DetachListener(); + } +} + +void CPhysicsSpring::DetachListener() +{ + if ( !(m_pObjStart->CallbackFlags() & CALLBACK_NEVER_DELETED) ) + { + m_pObjStart->GetObject()->remove_listener_object( this ); + } + + if ( !(m_pObjEnd->CallbackFlags() & CALLBACK_NEVER_DELETED) ) + { + m_pObjEnd->GetObject()->remove_listener_object( this ); + } + + m_pObjStart = NULL; + m_pObjEnd = NULL; + m_pSpring = NULL; +} + +void CPhysicsSpring::event_object_deleted( IVP_Event_Object * ) +{ + // the spring is going to delete itself now, so NULL it. + DetachListener(); +} + +void CPhysicsSpring::GetEndpoints( Vector* worldPositionStart, Vector* worldPositionEnd ) +{ + Vector localHL; + + if ( !m_pSpring ) + return; + + if ( worldPositionStart ) + { + const IVP_Anchor *anchor = m_pSpring->get_actuator_anchor(0); + ConvertPositionToHL( anchor->object_pos, localHL ); + m_pObjStart->LocalToWorld( worldPositionStart, localHL ); + } + + if ( worldPositionEnd ) + { + const IVP_Anchor *anchor = m_pSpring->get_actuator_anchor(1); + ConvertPositionToHL( anchor->object_pos, localHL ); + m_pObjEnd->LocalToWorld( worldPositionEnd, localHL ); + } +} + +void CPhysicsSpring::SetSpringConstant( float flSpringConstant ) +{ + if ( m_pSpring ) + { + float currentConstant = m_pSpring->get_constant(); + if ( currentConstant != flSpringConstant ) + { + m_pSpring->set_constant(flSpringConstant); + } + } +} + +void CPhysicsSpring::SetSpringDamping( float flSpringDamping ) +{ + if ( m_pSpring ) + { + m_pSpring->set_damp(flSpringDamping); + } +} + + + +void CPhysicsSpring::SetSpringLength( float flSpringLength ) +{ + if ( m_pSpring ) + { + float currentLengthIVP = m_pSpring->get_spring_length_zero_force(); + float desiredLengthIVP = ConvertDistanceToIVP(flSpringLength); + + // must change enough, or skip to keep objects sleeping + const float SPRING_LENGTH_EPSILON = 1e-3f; + if ( fabs(desiredLengthIVP-currentLengthIVP) < ConvertDistanceToIVP(SPRING_LENGTH_EPSILON) ) + return; + + m_pSpring->set_len( desiredLengthIVP ); + } +} + +void CPhysicsSpring::WriteToTemplate( vphysics_save_cphysicsspring_t ¶ms ) +{ + if ( !m_pSpring ) + { + memset(¶ms, 0, sizeof(params)); + return; + } + params.constant = m_pSpring->get_constant(); + params.naturalLength = ConvertDistanceToHL( m_pSpring->get_spring_length_zero_force() ); + params.damping = m_pSpring->get_damp_factor(); + params.relativeDamping = m_pSpring->get_rel_pos_damp(); + + const IVP_Anchor *anchor0 = m_pSpring->get_actuator_anchor(0); + ConvertPositionToHL( anchor0->object_pos, params.startPosition ); + const IVP_Anchor *anchor1 = m_pSpring->get_actuator_anchor(1); + ConvertPositionToHL( anchor1->object_pos, params.endPosition ); + params.useLocalPositions = true; + + params.onlyStretch = m_pSpring->get_only_stretch() ? true : false; + params.pObjStart = m_pObjStart; + params.pObjEnd = m_pObjEnd; +} + + +IPhysicsSpring *CreateSpring( IVP_Environment *pEnvironment, CPhysicsObject *pObjectStart, CPhysicsObject *pObjectEnd, springparams_t *pParams ) +{ + // fill in template + IVP_Template_Spring spring_template; + + spring_template.spring_values_are_relative=IVP_FALSE; + spring_template.spring_constant = pParams->constant; + spring_template.spring_len = ConvertDistanceToIVP( pParams->naturalLength ); + spring_template.spring_damp = pParams->damping; + spring_template.rel_pos_damp = pParams->relativeDamping; + + spring_template.spring_force_only_on_stretch = pParams->onlyStretch ? IVP_TRUE : IVP_FALSE; + + // Create anchors for the objects + IVP_Template_Anchor anchorTemplateObjectStart, anchorTemplateObjectEnd; + // create spring + + IVP_U_Float_Point ivpPosStart; + IVP_U_Float_Point ivpPosEnd; + + if ( !pParams->useLocalPositions ) + { + Vector local; + pObjectStart->WorldToLocal( &local, pParams->startPosition ); + ConvertPositionToIVP( local, ivpPosStart ); + + pObjectEnd->WorldToLocal( &local, pParams->endPosition ); + ConvertPositionToIVP( local, ivpPosEnd ); + } + else + { + ConvertPositionToIVP( pParams->startPosition, ivpPosStart ); + ConvertPositionToIVP( pParams->endPosition, ivpPosEnd ); + } + + anchorTemplateObjectStart.set_anchor_position_os( pObjectStart->GetObject(), &ivpPosStart ); + anchorTemplateObjectEnd.set_anchor_position_os( pObjectEnd->GetObject(), &ivpPosEnd ); + + spring_template.anchors[0] = &anchorTemplateObjectStart; + spring_template.anchors[1] = &anchorTemplateObjectEnd; + IVP_Actuator_Spring *pSpring = pEnvironment->create_spring( &spring_template ); + + return new CPhysicsSpring( pObjectStart, pObjectEnd, pSpring ); +} + + +bool SavePhysicsSpring( const physsaveparams_t ¶ms, CPhysicsSpring *pSpring ) +{ + vphysics_save_cphysicsspring_t springTemplate; + memset( &springTemplate, 0, sizeof(springTemplate) ); + + pSpring->WriteToTemplate( springTemplate ); + params.pSave->WriteAll( &springTemplate ); + return true; +} + +bool RestorePhysicsSpring( const physrestoreparams_t ¶ms, CPhysicsSpring **ppSpring ) +{ + vphysics_save_cphysicsspring_t springTemplate; + memset( &springTemplate, 0, sizeof(springTemplate) ); + params.pRestore->ReadAll( &springTemplate ); + CPhysicsEnvironment *pEnvironment = (CPhysicsEnvironment *)params.pEnvironment; + if ( springTemplate.pObjStart && springTemplate.pObjEnd ) + { + *ppSpring = (CPhysicsSpring *)pEnvironment->CreateSpring( springTemplate.pObjStart, springTemplate.pObjEnd, &springTemplate ); + } + else + { + DevMsg( "Failed to restore spring enpoints\n"); + *ppSpring = NULL; + } + + return true; +} + diff --git a/vphysics/physics_spring.h b/vphysics/physics_spring.h new file mode 100644 index 0000000..a8921d3 --- /dev/null +++ b/vphysics/physics_spring.h @@ -0,0 +1,22 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_SPRING_H +#define PHYSICS_SPRING_H +#pragma once + + +class IPhysicsSpring; +class IVP_Environment; +class IVP_Real_Object; + +struct springparams_t; + +IPhysicsSpring *CreateSpring( IVP_Environment *pEnvironment, CPhysicsObject *pObjectStart, CPhysicsObject *pObjectEnd, springparams_t *pParams ); + + +#endif // PHYSICS_SPRING_H diff --git a/vphysics/physics_trace.h b/vphysics/physics_trace.h new file mode 100644 index 0000000..ca24d76 --- /dev/null +++ b/vphysics/physics_trace.h @@ -0,0 +1,244 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_TRACE_H +#define PHYSICS_TRACE_H +#ifdef _WIN32 +#pragma once +#endif + + +class Vector; +class QAngle; +class CGameTrace; +class CTraceRay; +class IVP_Compact_Surface; +typedef CGameTrace trace_t; +struct Ray_t; +class IVP_Compact_Surface; +class IVP_Compact_Mopp; +class IConvexInfo; +enum +{ + COLLIDE_POLY = 0, + COLLIDE_MOPP = 1, + COLLIDE_BALL = 2, + COLLIDE_VIRTUAL = 3, +}; + +class IPhysCollide +{ +public: + virtual ~IPhysCollide() {} + //virtual void AddReference() = 0; + //virtual void ReleaseReference() = 0; + + // get a surface manager + virtual IVP_SurfaceManager *CreateSurfaceManager( short & ) const = 0; + virtual void GetAllLedges( IVP_U_BigVector<IVP_Compact_Ledge> &ledges ) const = 0; + virtual unsigned int GetSerializationSize() const = 0; + virtual unsigned int SerializeToBuffer( char *pDest, bool bSwap = false ) const = 0; + virtual int GetVCollideIndex() const = 0; + virtual Vector GetMassCenter() const = 0; + virtual void SetMassCenter( const Vector &massCenter ) = 0; + virtual Vector GetOrthographicAreas() const = 0; + virtual void SetOrthographicAreas( const Vector &areas ) = 0; + virtual float GetSphereRadius() const = 0; + virtual void OutputDebugInfo() const = 0; +}; + +#define LEAFMAP_HAS_CUBEMAP 0x0001 +#define LEAFMAP_HAS_SINGLE_VERTEX_SPAN 0x0002 +#define LEAFMAP_HAS_MULTIPLE_VERTEX_SPANS 0x0004 +struct leafmap_t +{ + void *pLeaf; + unsigned short vertCount; + byte flags; + byte spanCount; + unsigned short startVert[8]; + + void SetHasCubemap() + { + flags = LEAFMAP_HAS_CUBEMAP; + } + + void SetSingleVertexSpan( int startVertIndex, int vertCountIn ) + { + flags = 0; + flags |= LEAFMAP_HAS_SINGLE_VERTEX_SPAN; + startVert[0] = startVertIndex; + vertCount = vertCountIn; + } + + int MaxSpans() + { + return sizeof(startVert) - sizeof(startVert[0]); + } + const byte *GetSpans() const + { + return reinterpret_cast<const byte *>(&startVert[1]); + } + byte *GetSpans() + { + return reinterpret_cast<byte *>(&startVert[1]); + } + + void SetRLESpans( int startVertIndex, int spanCountIn, byte *pSpans ) + { + flags = 0; + if ( spanCountIn > MaxSpans() ) + return; + if ( spanCountIn == 1 ) + { + SetSingleVertexSpan( startVertIndex, pSpans[0] ); + return; + } + // write out a run length encoded list of verts to include in this model + flags |= LEAFMAP_HAS_MULTIPLE_VERTEX_SPANS; + startVert[0] = startVertIndex; + vertCount = 0; + spanCount = spanCountIn; + byte *pSpanOut = GetSpans(); + for ( int i = 0; i < spanCountIn; i++ ) + { + pSpanOut[i] = pSpans[i]; + if ( !(i & 1) ) + { + vertCount += pSpans[i]; + } + } + } + + inline bool HasSpans() const { return (flags & (LEAFMAP_HAS_SINGLE_VERTEX_SPAN|LEAFMAP_HAS_MULTIPLE_VERTEX_SPANS)) ? true : false; } + inline bool HasCubemap() const { return (flags & LEAFMAP_HAS_CUBEMAP) ? true : false; } + inline bool HasSingleVertexSpan() const { return (flags & LEAFMAP_HAS_SINGLE_VERTEX_SPAN) ? true : false; } + inline bool HasRLESpans() const { return (flags & LEAFMAP_HAS_MULTIPLE_VERTEX_SPANS) ? true : false; } +}; + +struct collidemap_t +{ + int leafCount; + leafmap_t leafmap[1]; +}; + +extern void InitLeafmap( IVP_Compact_Ledge *pLeaf, leafmap_t *pLeafmapOut ); + +class CPhysCollide : public IPhysCollide +{ +public: + static CPhysCollide *UnserializeFromBuffer( const char *pBuffer, unsigned int size, int index, bool swap = false ); + virtual const IVP_Compact_Surface *GetCompactSurface() const { return NULL; } + virtual Vector GetOrthographicAreas() const { return Vector(1,1,1); } + virtual float GetSphereRadius() const { return 0; } + virtual void ComputeOrthographicAreas( float epsilon ) {} + virtual void SetOrthographicAreas( const Vector &areas ) {} + virtual const collidemap_t *GetCollideMap() const { return NULL; } +}; + +class ITraceObject +{ +public: + virtual int SupportMap( const Vector &dir, Vector *pOut ) const = 0; + virtual Vector GetVertByIndex( int index ) const = 0; + virtual float Radius( void ) const = 0; +}; + +// This is the size of the vertex hash +#define CONVEX_HASH_SIZE 512 +// The little hashing trick below allows 64K verts per hash entry +#define MAX_CONVEX_VERTS ((CONVEX_HASH_SIZE * (1<<16))-1) + +class CPhysicsTrace +{ +public: + CPhysicsTrace(); + ~CPhysicsTrace(); + // Calculate the intersection of a swept box (mins/maxs) against an IVP object. All coords are in HL space. + void SweepBoxIVP( const Vector &start, const Vector &end, const Vector &mins, const Vector &maxs, const CPhysCollide *pSurface, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr ); + void SweepBoxIVP( const Ray_t &raySrc, unsigned int contentsMask, IConvexInfo *pConvexInfo, const CPhysCollide *pSurface, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr ); + + // Calculate the intersection of a swept compact surface against another compact surface. All coords are in HL space. + // NOTE: BUGBUG: swept surface must be single convex!!! + void SweepIVP( const Vector &start, const Vector &end, const CPhysCollide *pSweptSurface, const QAngle &sweptAngles, const CPhysCollide *pSurface, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr ); + + // get an AABB for an oriented collide + void GetAABB( Vector *pMins, Vector *pMaxs, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles ); + + // get the support map/extent for a collide along the axis given by "direction" + Vector GetExtent( const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, const Vector &direction ); + + bool IsBoxIntersectingCone( const Vector &boxAbsMins, const Vector &boxAbsMaxs, const truncatedcone_t &cone ); +}; + + +class CVisitHash +{ +public: + CVisitHash(); + inline unsigned short VertIndexToID( int vertIndex ); + inline void VisitVert( int vertIndex ); + inline bool WasVisited( int vertIndex ); + inline void NewVisit( void ); + +private: + + // Store the current increment and the vertex ID (rotating hash) to guarantee no collisions + struct vertmarker_t + { + unsigned short visitID; + unsigned short vertID; + }; + + vertmarker_t m_vertVisit[CONVEX_HASH_SIZE]; + unsigned short m_vertVisitID; + unsigned short m_isInUse; +}; + +// Calculate the intersection of a swept box (mins/maxs) against an IVP object. All coords are in HL space. +inline unsigned short CVisitHash::VertIndexToID( int vertIndex ) +{ + // A little hashing trick here: + // rotate the hash key each time you wrap around at 64K + // That way, the index will not collide until you've hit 64K # hash entries times + int high = vertIndex >> 16; + return (unsigned short) ((vertIndex + high) & 0xFFFF); +} + +inline void CVisitHash::VisitVert( int vertIndex ) +{ + int index = vertIndex & (CONVEX_HASH_SIZE-1); + m_vertVisit[index].visitID = m_vertVisitID; + m_vertVisit[index].vertID = VertIndexToID(vertIndex); +} + +inline bool CVisitHash::WasVisited( int vertIndex ) +{ + unsigned short hashIndex = vertIndex & (CONVEX_HASH_SIZE-1); + unsigned short id = VertIndexToID(vertIndex); + if ( m_vertVisit[hashIndex].visitID == m_vertVisitID && m_vertVisit[hashIndex].vertID == id ) + return true; + + return false; +} + +inline void CVisitHash::NewVisit( void ) +{ + m_vertVisitID++; + if ( m_vertVisitID == 0 ) + { + memset( m_vertVisit, 0, sizeof(m_vertVisit) ); + } + +} + + + +extern IVP_SurfaceManager *CreateSurfaceManager( const CPhysCollide *pCollisionModel, short &collideType ); +extern void OutputCollideDebugInfo( const CPhysCollide *pCollisionModel ); + +#endif // PHYSICS_TRACE_H diff --git a/vphysics/physics_vehicle.cpp b/vphysics/physics_vehicle.cpp new file mode 100644 index 0000000..1c46798 --- /dev/null +++ b/vphysics/physics_vehicle.cpp @@ -0,0 +1,1606 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// +#ifdef _WIN32 +#pragma warning (disable:4127) +#pragma warning (disable:4244) +#endif + +#include "cbase.h" +#include "ivp_controller.hxx" +#include "ivp_cache_object.hxx" +#include "ivp_car_system.hxx" +#include "ivp_constraint_car.hxx" +#include "ivp_material.hxx" + +#include "vphysics/vehicles.h" +#include "vphysics/friction.h" +#include "physics_vehicle.h" +#include "physics_controller_raycast_vehicle.h" +#include "physics_airboat.h" +#include "ivp_car_system.hxx" +#include "ivp_listener_object.hxx" + +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +#define THROTTLE_OPPOSING_FORCE_EPSILON 5.0f +#define VEHICLE_SKID_EPSILON 0.1f + +// y in/s = x miles/hour * (5280 * 12 (in / mile)) * (1 / 3600 (hour / sec) ) +//#define MPH2INS(x) ( (x) * 5280.0f * 12.0f / 3600.0f ) +//#define INS2MPH(x) ( (x) * 3600 * (1/5280.0f) * (1/12.0f) ) + +#define MPH_TO_METERSPERSECOND 0.44707f +#define METERSPERSECOND_TO_MPH (1.0f / MPH_TO_METERSPERSECOND) + +#define MPH_TO_GAMEVEL(x) (ConvertDistanceToHL( (x) * MPH_TO_METERSPERSECOND )) +#define GAMEVEL_TO_MPH(x) (ConvertDistanceToIVP(x) * METERSPERSECOND_TO_MPH) + + +#define FVEHICLE_THROTTLE_STOPPED 0x00000001 +#define FVEHICLE_HANDBRAKE_ON 0x00000002 + +struct vphysics_save_cvehiclecontroller_t +{ + CPhysicsObject *m_pCarBody; + int m_wheelCount; + vehicleparams_t m_vehicleData; + vehicle_operatingparams_t m_currentState; + float m_wheelRadius; + float m_bodyMass; + float m_totalWheelMass; + float m_gravityLength; + float m_torqueScale; + CPhysicsObject *m_pWheels[VEHICLE_MAX_WHEEL_COUNT]; + Vector m_wheelPosition_Bs[VEHICLE_MAX_WHEEL_COUNT]; + Vector m_tracePosition_Bs[VEHICLE_MAX_WHEEL_COUNT]; + int m_vehicleFlags; + unsigned int m_nTireType; + unsigned int m_nVehicleType; + bool m_bTraceData; + bool m_bOccupied; + bool m_bEngineDisable; + float m_flVelocity[3]; + + DECLARE_SIMPLE_DATADESC(); +}; + + +BEGIN_SIMPLE_DATADESC( vphysics_save_cvehiclecontroller_t ) + DEFINE_VPHYSPTR( m_pCarBody ), + DEFINE_FIELD( m_wheelCount, FIELD_INTEGER ), + DEFINE_EMBEDDED( m_vehicleData ), + DEFINE_EMBEDDED( m_currentState ), + DEFINE_FIELD( m_wheelCount, FIELD_INTEGER ), + DEFINE_FIELD( m_bodyMass, FIELD_FLOAT ), + DEFINE_FIELD( m_totalWheelMass, FIELD_FLOAT ), + DEFINE_FIELD( m_gravityLength, FIELD_FLOAT ), + DEFINE_FIELD( m_torqueScale, FIELD_FLOAT ), + + DEFINE_VPHYSPTR_ARRAY( m_pWheels, VEHICLE_MAX_WHEEL_COUNT ), + DEFINE_ARRAY( m_wheelPosition_Bs, FIELD_VECTOR, VEHICLE_MAX_WHEEL_COUNT ), + DEFINE_ARRAY( m_tracePosition_Bs, FIELD_VECTOR, VEHICLE_MAX_WHEEL_COUNT ), + DEFINE_FIELD( m_vehicleFlags, FIELD_INTEGER ), + DEFINE_FIELD( m_nTireType, FIELD_INTEGER ), + DEFINE_FIELD( m_nVehicleType, FIELD_INTEGER ), + DEFINE_FIELD( m_bTraceData, FIELD_BOOLEAN ), + DEFINE_FIELD( m_bOccupied, FIELD_BOOLEAN ), + DEFINE_FIELD( m_bEngineDisable, FIELD_BOOLEAN ), + DEFINE_ARRAY( m_flVelocity, FIELD_FLOAT, 3 ), +END_DATADESC() + +BEGIN_SIMPLE_DATADESC( vehicle_operatingparams_t ) + DEFINE_FIELD( speed, FIELD_FLOAT ), + DEFINE_FIELD( engineRPM, FIELD_FLOAT ), + DEFINE_FIELD( gear, FIELD_INTEGER ), + DEFINE_FIELD( boostDelay, FIELD_FLOAT ), + DEFINE_FIELD( boostTimeLeft, FIELD_INTEGER ), + DEFINE_FIELD( skidSpeed, FIELD_FLOAT ), + DEFINE_CUSTOM_FIELD( skidMaterial, MaterialIndexDataOps() ), + DEFINE_FIELD( steeringAngle, FIELD_FLOAT ), + DEFINE_FIELD( wheelsInContact, FIELD_INTEGER ), + DEFINE_FIELD( wheelsNotInContact,FIELD_INTEGER ), + DEFINE_FIELD( isTorqueBoosting, FIELD_BOOLEAN ), +END_DATADESC() + +BEGIN_SIMPLE_DATADESC( vehicle_bodyparams_t ) + DEFINE_FIELD( massCenterOverride, FIELD_VECTOR ), + DEFINE_FIELD( massOverride, FIELD_FLOAT ), + DEFINE_FIELD( addGravity, FIELD_FLOAT ), + DEFINE_FIELD( maxAngularVelocity, FIELD_FLOAT ), + DEFINE_FIELD( tiltForce, FIELD_FLOAT ), + DEFINE_FIELD( tiltForceHeight, FIELD_FLOAT ), + DEFINE_FIELD( counterTorqueFactor, FIELD_FLOAT ), + DEFINE_FIELD( keepUprightTorque, FIELD_FLOAT ), +END_DATADESC() + +BEGIN_SIMPLE_DATADESC( vehicle_wheelparams_t ) + DEFINE_FIELD( radius, FIELD_FLOAT ), + DEFINE_FIELD( mass, FIELD_FLOAT ), + DEFINE_FIELD( inertia, FIELD_FLOAT ), + DEFINE_FIELD( damping, FIELD_FLOAT ), + DEFINE_FIELD( rotdamping, FIELD_FLOAT ), + DEFINE_FIELD( frictionScale, FIELD_FLOAT ), + DEFINE_CUSTOM_FIELD( materialIndex, MaterialIndexDataOps() ), + DEFINE_CUSTOM_FIELD( brakeMaterialIndex, MaterialIndexDataOps() ), + DEFINE_CUSTOM_FIELD( skidMaterialIndex, MaterialIndexDataOps() ), + DEFINE_FIELD( springAdditionalLength, FIELD_FLOAT ), +END_DATADESC() + +BEGIN_SIMPLE_DATADESC( vehicle_suspensionparams_t ) + DEFINE_FIELD( springConstant, FIELD_FLOAT ), + DEFINE_FIELD( springDamping, FIELD_FLOAT ), + DEFINE_FIELD( stabilizerConstant, FIELD_FLOAT ), + DEFINE_FIELD( springDampingCompression, FIELD_FLOAT ), + DEFINE_FIELD( maxBodyForce, FIELD_FLOAT ), +END_DATADESC() + +BEGIN_SIMPLE_DATADESC( vehicle_axleparams_t ) + DEFINE_FIELD( offset, FIELD_VECTOR ), + DEFINE_FIELD( wheelOffset, FIELD_VECTOR ), + DEFINE_FIELD( raytraceCenterOffset, FIELD_VECTOR ), + DEFINE_FIELD( raytraceOffset, FIELD_VECTOR ), + DEFINE_EMBEDDED( wheels ), + DEFINE_EMBEDDED( suspension ), + DEFINE_FIELD( torqueFactor, FIELD_FLOAT ), + DEFINE_FIELD( brakeFactor, FIELD_FLOAT ), +END_DATADESC() + +BEGIN_SIMPLE_DATADESC( vehicle_steeringparams_t ) + DEFINE_FIELD( degreesSlow, FIELD_FLOAT ), + DEFINE_FIELD( degreesFast, FIELD_FLOAT ), + DEFINE_FIELD( degreesBoost, FIELD_FLOAT ), + DEFINE_FIELD( steeringRateSlow, FIELD_FLOAT ), + DEFINE_FIELD( steeringRateFast, FIELD_FLOAT ), + DEFINE_FIELD( steeringRestRateSlow, FIELD_FLOAT ), + DEFINE_FIELD( steeringRestRateFast, FIELD_FLOAT ), + DEFINE_FIELD( throttleSteeringRestRateFactor, FIELD_FLOAT ), + DEFINE_FIELD( boostSteeringRestRateFactor, FIELD_FLOAT ), + DEFINE_FIELD( boostSteeringRateFactor, FIELD_FLOAT ), + DEFINE_FIELD( steeringExponent, FIELD_FLOAT ), + DEFINE_FIELD( speedSlow, FIELD_FLOAT ), + DEFINE_FIELD( speedFast, FIELD_FLOAT ), + DEFINE_FIELD( turnThrottleReduceSlow, FIELD_FLOAT ), + DEFINE_FIELD( turnThrottleReduceFast, FIELD_FLOAT ), + DEFINE_FIELD( powerSlideAccel, FIELD_FLOAT ), + DEFINE_FIELD( brakeSteeringRateFactor, FIELD_FLOAT ), + DEFINE_FIELD( isSkidAllowed, FIELD_BOOLEAN ), + DEFINE_FIELD( dustCloud, FIELD_BOOLEAN ), +END_DATADESC() + +BEGIN_SIMPLE_DATADESC( vehicle_engineparams_t ) + DEFINE_FIELD( horsepower, FIELD_FLOAT ), + DEFINE_FIELD( maxSpeed, FIELD_FLOAT ), + DEFINE_FIELD( maxRevSpeed, FIELD_FLOAT ), + DEFINE_FIELD( maxRPM, FIELD_FLOAT ), + DEFINE_FIELD( axleRatio, FIELD_FLOAT ), + DEFINE_FIELD( throttleTime, FIELD_FLOAT ), + DEFINE_FIELD( maxRPM, FIELD_FLOAT ), + DEFINE_FIELD( isAutoTransmission, FIELD_BOOLEAN ), + DEFINE_FIELD( gearCount, FIELD_INTEGER ), + DEFINE_AUTO_ARRAY( gearRatio, FIELD_FLOAT ), + DEFINE_FIELD( shiftUpRPM, FIELD_FLOAT ), + DEFINE_FIELD( shiftDownRPM, FIELD_FLOAT ), + DEFINE_FIELD( boostForce, FIELD_FLOAT ), + DEFINE_FIELD( boostDuration, FIELD_FLOAT ), + DEFINE_FIELD( boostDelay, FIELD_FLOAT ), + DEFINE_FIELD( boostMaxSpeed, FIELD_FLOAT ), + DEFINE_FIELD( autobrakeSpeedGain, FIELD_FLOAT ), + DEFINE_FIELD( autobrakeSpeedFactor, FIELD_FLOAT ), + DEFINE_FIELD( torqueBoost, FIELD_BOOLEAN ), +END_DATADESC() + +BEGIN_SIMPLE_DATADESC( vehicleparams_t ) + DEFINE_FIELD( axleCount, FIELD_INTEGER ), + DEFINE_FIELD( wheelsPerAxle, FIELD_INTEGER ), + DEFINE_EMBEDDED( body ), + DEFINE_EMBEDDED_AUTO_ARRAY( axles ), + DEFINE_EMBEDDED( engine ), + DEFINE_EMBEDDED( steering ), +END_DATADESC() + + +bool IsVehicleWheel( IVP_Real_Object *pivp ) +{ + CPhysicsObject *pObject = static_cast<CPhysicsObject *>(pivp->client_data); + + // FIXME: Check why this is null! It occurs when jumping the ravine in seafloor + if (!pObject) + return false; + + return (pObject->CallbackFlags() & CALLBACK_IS_VEHICLE_WHEEL) ? true : false; +} + +inline bool IsMoveable( IVP_Real_Object *pObject ) +{ + IVP_Core *pCore = pObject->get_core(); + if ( pCore->pinned || pCore->physical_unmoveable ) + return false; + return true; +} + +inline bool IVPFloatPointIsZero( const IVP_U_Float_Point &test ) +{ + const float eps = 1e-4f; + return test.quad_length() < eps ? true : false; +} + +bool ShouldOverrideWheelContactFriction( float *pFrictionOut, IVP_Real_Object *pivp0, IVP_Real_Object *pivp1, IVP_U_Float_Point *pNormal ) +{ + if ( !pivp0->get_core()->car_wheel && !pivp1->get_core()->car_wheel ) + return false; + + if ( !IsVehicleWheel(pivp0) ) + { + if ( !IsVehicleWheel(pivp1) ) + return false; + // swap so pivp0 is a wheel + IVP_Real_Object *pTmp = pivp0; + pivp0 = pivp1; + pivp1 = pTmp; + } + + // if we got here then pivp0 is a car wheel object + // BUGBUG: IVP sometimes sends us a bogus normal + // when doing a material realc on existing contacts! + if ( !IVPFloatPointIsZero(pNormal) ) + { + IVP_U_Float_Point normalWheelSpace; + pivp0->get_core()->get_m_world_f_core_PSI()->vimult3( pNormal, &normalWheelSpace ); + if ( fabs(normalWheelSpace.k[0]) > 0.2588f ) // 15 degree wheel cone + { + // adjust friction here, this isn't a valid part of the wheel for contact, set friction to zero + //Vector tmp;ConvertDirectionToHL( normalWheelSpace, tmp );Msg("Wheel sliding on surface %.2f %.2f %.2f\n", tmp.x, tmp.y, tmp.z ); + *pFrictionOut = 0; + return true; + } + } + // was car wheel, but didn't adjust - use default friction + return false; +} + + +class CVehicleController : public IPhysicsVehicleController, public IVP_Listener_Object +{ +public: + CVehicleController( ); + CVehicleController( const vehicleparams_t ¶ms, CPhysicsEnvironment *pEnv, unsigned int nVehicleType, IPhysicsGameTrace *pGameTrace ); + ~CVehicleController(); + + // CVehicleController + void InitCarSystem( CPhysicsObject *pBodyObject ); + + // IPhysicsVehicleController + void Update( float dt, vehicle_controlparams_t &controls ); + float UpdateBooster( float dt ); + void SetSpringLength(int wheelIndex, float length); + const vehicle_operatingparams_t &GetOperatingParams() { return m_currentState; } + const vehicleparams_t &GetVehicleParams() { return m_vehicleData; } + vehicleparams_t &GetVehicleParamsForChange() { return m_vehicleData; } + int GetWheelCount(void) { return m_wheelCount; }; + IPhysicsObject* GetWheel(int index); + virtual bool GetWheelContactPoint( int index, Vector *pContactPoint, int *pSurfaceProps ); + void SetWheelFriction(int wheelIndex, float friction); + + void SetEngineDisabled( bool bDisable ) { m_bEngineDisable = bDisable; } + bool IsEngineDisabled( void ) { return m_bEngineDisable; } + + // Save/load + void WriteToTemplate( vphysics_save_cvehiclecontroller_t &controllerTemplate ); + void InitFromTemplate( CPhysicsEnvironment *pEnv, void *pGameData, IPhysicsGameTrace *pGameTrace, const vphysics_save_cvehiclecontroller_t &controllerTemplate ); + void VehicleDataReload(); + + // Debug + void GetCarSystemDebugData( vehicle_debugcarsystem_t &debugCarSystem ); + + // IVP_Listener_Object + // Object listener, only hook delete + virtual void event_object_deleted( IVP_Event_Object *); + virtual void event_object_created( IVP_Event_Object *) {} + virtual void event_object_revived( IVP_Event_Object *) {} + virtual void event_object_frozen ( IVP_Event_Object *) {} + + // Entry/Exit + void OnVehicleEnter( void ); + void OnVehicleExit( void ); + +protected: + void CreateIVPObjects( ); + void ShutdownCarSystem(); + + void InitVehicleData( const vehicleparams_t ¶ms ); + void InitCarSystemBody( IVP_Template_Car_System &ivpVehicleData ); + void InitCarSystemWheels( IVP_Template_Car_System &ivpVehicleData ); + + void AttachListener(); + + IVP_Real_Object *CreateWheel( int wheelIndex, vehicle_axleparams_t &axle ); + void CreateTraceData( int wheelIndex, vehicle_axleparams_t &axle ); + + // Update. + void UpdateSteering( const vehicle_controlparams_t &controls, float flDeltaTime, float flSpeed ); + void UpdatePowerslide( const vehicle_controlparams_t &controls, bool bPowerslide, float flSpeed ); + void UpdateEngine( const vehicle_controlparams_t &controls, float flDeltaTime, float flThrottle, float flBrake, bool bHandbrake, bool bPowerslide ); + bool UpdateEngineTurboStart( const vehicle_controlparams_t &controls, float flDeltaTime ); + void UpdateEngineTurboFinish( void ); + void UpdateHandbrake( const vehicle_controlparams_t &controls, float flThrottle, bool bHandbrake, bool bPowerslide ); + void UpdateSkidding( bool bHandbrake ); + void UpdateExtraForces( void ); + void UpdateWheelPositions( void ); + float CalcSteering( float dt, float speed, float steering, bool bAnalog ); + void CalcEngine( float throttle, float brake_val, bool handbrake, float steeringVal, bool torqueBoost ); + void CalcEngineTransmission( float flThrottle ); + + virtual bool IsBoosting( void ); + +private: + void ResetState(); + IVP_Car_System *m_pCarSystem; + CPhysicsObject *m_pCarBody; + CPhysicsEnvironment *m_pEnv; + IPhysicsGameTrace *m_pGameTrace; + int m_wheelCount; + vehicleparams_t m_vehicleData; + vehicle_operatingparams_t m_currentState; + float m_wheelRadius; + float m_bodyMass; + float m_totalWheelMass; + float m_gravityLength; + float m_torqueScale; + CPhysicsObject *m_pWheels[VEHICLE_MAX_WHEEL_COUNT]; + IVP_U_Float_Point m_wheelPosition_Bs[VEHICLE_MAX_WHEEL_COUNT]; + IVP_U_Float_Point m_tracePosition_Bs[VEHICLE_MAX_WHEEL_COUNT]; + int m_vehicleFlags; + unsigned int m_nTireType; + unsigned int m_nVehicleType; + bool m_bTraceData; + bool m_bOccupied; + bool m_bEngineDisable; + float m_flVelocity[3]; +}; + +CVehicleController::CVehicleController( const vehicleparams_t ¶ms, CPhysicsEnvironment *pEnv, unsigned int nVehicleType, IPhysicsGameTrace *pGameTrace ) +{ + m_pEnv = pEnv; + m_pGameTrace = pGameTrace; + m_nVehicleType = nVehicleType; + InitVehicleData( params ); + ResetState(); +} + + +CVehicleController::CVehicleController() +{ + ResetState(); +} + + +void CVehicleController::ResetState() +{ + m_pCarSystem = NULL; + m_flVelocity[0] = m_flVelocity[1]= m_flVelocity[2] = 0.0f; + for ( int i = 0; i < VEHICLE_MAX_WHEEL_COUNT; i++ ) + { + m_pWheels[i] = NULL; + } + m_pCarBody = NULL; + m_torqueScale = 1; + m_wheelCount = 0; + m_wheelRadius = 0; + memset( &m_currentState, 0, sizeof(m_currentState) ); + m_bodyMass = 0; + m_vehicleFlags = 0; + memset( m_wheelPosition_Bs, 0, sizeof(m_wheelPosition_Bs) ); + memset( m_tracePosition_Bs, 0, sizeof(m_tracePosition_Bs) ); + + m_bTraceData = false; + if ( m_nVehicleType == VEHICLE_TYPE_AIRBOAT_RAYCAST ) + { + m_bTraceData = true; + } + + m_nTireType = VEHICLE_TIRE_NORMAL; + + m_bOccupied = false; + m_bEngineDisable = false; +} + +CVehicleController::~CVehicleController() +{ + ShutdownCarSystem(); +} + +IPhysicsObject* CVehicleController::GetWheel( int index ) +{ + // TODO: This is getting messy. + if ( m_nVehicleType == VEHICLE_TYPE_CAR_WHEELS ) + { + return m_pWheels[index]; + } + else if ( m_nVehicleType == VEHICLE_TYPE_CAR_RAYCAST && m_pCarSystem ) + { + return static_cast<CPhysics_Car_System_Raycast_Wheels*>( m_pCarSystem )->GetWheel( index ); + } + else if ( m_nVehicleType == VEHICLE_TYPE_AIRBOAT_RAYCAST && m_pCarSystem ) + { + return static_cast<CPhysics_Airboat*>( m_pCarSystem )->GetWheel( index ); + } + + return NULL; +} + +void CVehicleController::SetWheelFriction(int wheelIndex, float friction) +{ + CPhysics_Airboat *pAirboat = static_cast<CPhysics_Airboat*>( m_pCarSystem ); + if ( !pAirboat ) + return; + + pAirboat->SetWheelFriction( wheelIndex, friction ); +} + +bool CVehicleController::GetWheelContactPoint( int index, Vector *pContactPoint, int *pSurfaceProps ) +{ + bool bSet = false; + if ( index < m_wheelCount ) + { + IPhysicsFrictionSnapshot *pSnapshot = m_pWheels[index]->CreateFrictionSnapshot(); + float forceMax = -1.0f; + m_pWheels[index]->GetPosition( pContactPoint, NULL ); + while ( pSnapshot->IsValid() ) + { + float thisForce = pSnapshot->GetNormalForce(); + if ( thisForce > forceMax ) + { + forceMax = thisForce; + if ( pContactPoint ) + { + pSnapshot->GetContactPoint( *pContactPoint ); + } + if ( pSurfaceProps ) + { + *pSurfaceProps = pSnapshot->GetMaterial(1); + } + bSet = true; + } + pSnapshot->NextFrictionData(); + } + m_pWheels[index]->DestroyFrictionSnapshot(pSnapshot); + } + else + { + if ( pContactPoint ) + { + pContactPoint->Init(); + } + if ( pSurfaceProps ) + { + *pSurfaceProps = 0; + } + + } + return bSet; +} + +void CVehicleController::AttachListener() +{ + m_pCarBody->GetObject()->add_listener_object( this ); +} + +void CVehicleController::event_object_deleted( IVP_Event_Object *pEvent ) +{ + // the car system's constraint solver is going to delete itself now, so NULL the car system. + + m_pCarSystem->event_object_deleted( pEvent ); + m_pCarSystem = NULL; + ShutdownCarSystem(); +} + +IVP_Real_Object *CVehicleController::CreateWheel( int wheelIndex, vehicle_axleparams_t &axle ) +{ + if ( wheelIndex >= VEHICLE_MAX_WHEEL_COUNT ) + return NULL; + + // HACKHACK: In Save/load, the wheel was reloaded, so pretend to create it + // ALSO NOTE: Save/load puts the results into m_pWheels regardless of vehicle type!!! + // That's why I'm not calling GetWheel(). + if ( m_pWheels[wheelIndex] ) + { + CPhysicsObject *pWheelObject = static_cast<CPhysicsObject *>(m_pWheels[wheelIndex]); + return pWheelObject->GetObject(); + } + + objectparams_t params; + memset( ¶ms, 0, sizeof(params) ); + + Vector bodyPosition; + QAngle bodyAngles; + m_pCarBody->GetPosition( &bodyPosition, &bodyAngles ); + matrix3x4_t matrix; + AngleMatrix( bodyAngles, bodyPosition, matrix ); + + Vector position = axle.offset; + + // BUGBUG: This only works with 2 wheels per axle + if ( wheelIndex & 1 ) + { + position += axle.wheelOffset; + } + else + { + position -= axle.wheelOffset; + } + + Vector wheelPositionHL; + VectorTransform( position, matrix, wheelPositionHL ); + + params.damping = axle.wheels.damping; + params.dragCoefficient = 0; + params.enableCollisions = false; + params.inertia = axle.wheels.inertia; + params.mass = axle.wheels.mass; + params.pGameData = m_pCarBody->GetGameData(); + params.pName = "VehicleWheel"; + params.rotdamping = axle.wheels.rotdamping; + params.rotInertiaLimit = 0; + params.massCenterOverride = NULL; + // needs to be in HL units because we're calling through the "outer" interface to create + // the wheels + float radius = axle.wheels.radius; + float r3 = radius * radius * radius; + params.volume = (4 / 3) * M_PI * r3; + + CPhysicsObject *pWheel = (CPhysicsObject *)m_pEnv->CreateSphereObject( radius, axle.wheels.materialIndex, wheelPositionHL, bodyAngles, ¶ms, false ); + pWheel->Wake(); + + // UNDONE: only mask off some of these flags? + unsigned int flags = pWheel->CallbackFlags(); + flags = 0; + pWheel->SetCallbackFlags( flags ); + // copy the body's game flags + pWheel->SetGameFlags( m_pCarBody->GetGameFlags() ); + // cache the wheel object pointer + m_pWheels[wheelIndex] = pWheel; + + IVP_U_Point wheelPositionIVP, wheelPositionBs; + ConvertPositionToIVP( wheelPositionHL, wheelPositionIVP ); + TransformIVPToLocal( wheelPositionIVP, wheelPositionBs, m_pCarBody->GetObject(), true ); + m_wheelPosition_Bs[wheelIndex].set_to_zero(); + m_wheelPosition_Bs[wheelIndex].set( &wheelPositionBs ); + + pWheel->AddCallbackFlags( CALLBACK_IS_VEHICLE_WHEEL ); + + return pWheel->GetObject(); +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CVehicleController::CreateTraceData( int wheelIndex, vehicle_axleparams_t &axle ) +{ + if ( wheelIndex >= VEHICLE_MAX_WHEEL_COUNT ) + return; + + objectparams_t params; + memset( ¶ms, 0, sizeof( params ) ); + + Vector bodyPosition; + QAngle bodyAngles; + matrix3x4_t matrix; + m_pCarBody->GetPosition( &bodyPosition, &bodyAngles ); + AngleMatrix( bodyAngles, bodyPosition, matrix ); + + Vector tracePosition = axle.raytraceCenterOffset; + // BUGBUG: This only works with 2 wheels per axle + if ( wheelIndex & 1 ) + { + tracePosition += axle.raytraceOffset; + } + else + { + tracePosition -= axle.raytraceOffset; + } + + Vector tracePositionHL; + VectorTransform( tracePosition, matrix, tracePositionHL ); + + IVP_U_Point tracePositionIVP, tracePositionBs; + ConvertPositionToIVP( tracePositionHL, tracePositionIVP ); + TransformIVPToLocal( tracePositionIVP, tracePositionBs, m_pCarBody->GetObject(), true ); + m_tracePosition_Bs[wheelIndex].set_to_zero(); + m_tracePosition_Bs[wheelIndex].set( &tracePositionBs ); +} + +void CVehicleController::CreateIVPObjects( ) +{ + // Initialize the car system (body and wheels). + IVP_Template_Car_System ivpVehicleData( m_wheelCount, m_vehicleData.axleCount ); + InitCarSystemBody( ivpVehicleData ); + + InitCarSystemWheels( ivpVehicleData ); + + BEGIN_IVP_ALLOCATION(); + + // Raycast Car + switch ( m_nVehicleType ) + { + case VEHICLE_TYPE_CAR_WHEELS: + m_pCarSystem = new IVP_Car_System_Real_Wheels( m_pEnv->GetIVPEnvironment(), &ivpVehicleData ); + break + ; + case VEHICLE_TYPE_CAR_RAYCAST: + m_pCarSystem = new CPhysics_Car_System_Raycast_Wheels( m_pEnv->GetIVPEnvironment(), &ivpVehicleData ); + break; + + case VEHICLE_TYPE_AIRBOAT_RAYCAST: + m_pCarSystem = new CPhysics_Airboat( m_pEnv->GetIVPEnvironment(), &ivpVehicleData, m_pGameTrace ); + break; + } + + AttachListener(); + + END_IVP_ALLOCATION(); +} + + +void CVehicleController::InitCarSystem( CPhysicsObject *pBodyObject ) +{ + if ( m_pCarSystem ) + { + ShutdownCarSystem(); + } + + // Car body. + m_pCarBody = pBodyObject; + m_bodyMass = m_pCarBody->GetMass(); + m_gravityLength = m_pEnv->GetIVPEnvironment()->get_gravity()->real_length(); + // Setup axle/wheel counts. + m_wheelCount = m_vehicleData.axleCount * m_vehicleData.wheelsPerAxle; + CreateIVPObjects(); + + if ( m_nVehicleType == VEHICLE_TYPE_AIRBOAT_RAYCAST ) + { + float flDampSpeed = 1.0f; + float flDampRotSpeed = 1.0f; + m_pCarBody->SetDamping( &flDampSpeed, &flDampRotSpeed ); + } +} + +void CVehicleController::VehicleDataReload() +{ + // compute torque normalization factor + m_torqueScale = 1; + // Clear accumulation. + float totalTorqueDistribution = 0.0f; + for ( int i = 0; i < m_vehicleData.axleCount; i++ ) + { + totalTorqueDistribution += m_vehicleData.axles[i].torqueFactor; + } + + if ( totalTorqueDistribution > 0 ) + { + m_torqueScale /= totalTorqueDistribution; + } + // input speed is in miles/hour. Convert to in/s + m_vehicleData.engine.maxSpeed = MPH_TO_GAMEVEL(m_vehicleData.engine.maxSpeed); + m_vehicleData.engine.maxRevSpeed = MPH_TO_GAMEVEL(m_vehicleData.engine.maxRevSpeed); + m_vehicleData.engine.boostMaxSpeed = MPH_TO_GAMEVEL(m_vehicleData.engine.boostMaxSpeed); +} + + +//----------------------------------------------------------------------------- +// Purpose: Setup the body parameters. +//----------------------------------------------------------------------------- +void CVehicleController::InitCarSystemBody( IVP_Template_Car_System &ivpVehicleData ) +{ + ivpVehicleData.car_body = m_pCarBody->GetObject(); + + ivpVehicleData.index_x = IVP_INDEX_X; + ivpVehicleData.index_y = IVP_INDEX_Y; + ivpVehicleData.index_z = IVP_INDEX_Z; + + ivpVehicleData.body_counter_torque_factor = m_vehicleData.body.counterTorqueFactor; + ivpVehicleData.body_down_force_vertical_offset = ConvertDistanceToIVP( m_vehicleData.body.tiltForceHeight ); + ivpVehicleData.extra_gravity_force_value = m_vehicleData.body.addGravity * m_gravityLength * m_bodyMass; + ivpVehicleData.extra_gravity_height_offset = 0; + +#if 0 + // HACKHACK: match example + ivpVehicleData.extra_gravity_force_value = 1.2; + ivpVehicleData.body_down_force_vertical_offset = 2; +#endif +} + +//----------------------------------------------------------------------------- +// Purpose: Setup the wheel paramters. +//----------------------------------------------------------------------------- +void CVehicleController::InitCarSystemWheels( IVP_Template_Car_System &ivpVehicleData ) +{ + int wheelIndex = 0; + + m_wheelRadius = 0; + m_totalWheelMass = 0; + + int i; + for ( i = 0; i < m_vehicleData.axleCount; i++ ) + { + for ( int w = 0; w < m_vehicleData.wheelsPerAxle; w++, wheelIndex++ ) + { + IVP_Real_Object *pWheel = CreateWheel( wheelIndex, m_vehicleData.axles[i] ); + if ( pWheel ) + { + // Create ray trace data for wheel. + if ( m_bTraceData ) + { + CreateTraceData( wheelIndex, m_vehicleData.axles[i] ); + } + + ivpVehicleData.car_wheel[wheelIndex] = pWheel; + ivpVehicleData.wheel_radius[wheelIndex] = pWheel->get_core()->upper_limit_radius; + ivpVehicleData.wheel_reversed_sign[wheelIndex] = 1.0; + // only for raycast car + + ivpVehicleData.friction_of_wheel[wheelIndex] = m_vehicleData.axles[i].wheels.frictionScale; + ivpVehicleData.spring_constant[wheelIndex] = m_vehicleData.axles[i].suspension.springConstant * m_bodyMass; + ivpVehicleData.spring_dampening[wheelIndex] = m_vehicleData.axles[i].suspension.springDamping * m_bodyMass; + ivpVehicleData.spring_dampening_compression[wheelIndex] = m_vehicleData.axles[i].suspension.springDampingCompression * m_bodyMass; + ivpVehicleData.max_body_force[wheelIndex] = m_vehicleData.axles[i].suspension.maxBodyForce * m_bodyMass; + ivpVehicleData.spring_pre_tension[wheelIndex] = -ConvertDistanceToIVP( m_vehicleData.axles[i].wheels.springAdditionalLength ); + + ivpVehicleData.wheel_pos_Bos[wheelIndex] = m_wheelPosition_Bs[wheelIndex]; + if ( m_bTraceData ) + { + ivpVehicleData.trace_pos_Bos[wheelIndex] = m_tracePosition_Bs[wheelIndex]; + } + + m_totalWheelMass += m_vehicleData.axles[i].wheels.mass; + } + } + + ivpVehicleData.stabilizer_constant[i] = m_vehicleData.axles[i].suspension.stabilizerConstant * m_bodyMass; + // this should output in radians per second + float radius = ConvertDistanceToIVP( m_vehicleData.axles[i].wheels.radius ); + float totalMaxSpeed = max( m_vehicleData.engine.boostMaxSpeed, m_vehicleData.engine.maxSpeed ); + ivpVehicleData.wheel_max_rotation_speed[i] = totalMaxSpeed / radius; + if ( radius > m_wheelRadius ) + { + m_wheelRadius = radius; + } + } + + for ( i = 0; i < m_wheelCount; i++ ) + { + m_pWheels[i]->EnableCollisions( true ); + } +} + +void CVehicleController::ShutdownCarSystem() +{ + delete m_pCarSystem; + m_pCarSystem = NULL; + for ( int i = 0; i < m_wheelCount; i++ ) + { + if ( m_pWheels[i] ) + { + m_pEnv->DestroyObject( m_pWheels[i] ); + } + m_pWheels[i] = NULL; + } +} + + +void CVehicleController::InitVehicleData( const vehicleparams_t ¶ms ) +{ + m_vehicleData = params; + VehicleDataReload(); +} + +void CVehicleController::SetSpringLength(int wheelIndex, float length) +{ + m_pCarSystem->change_spring_length((IVP_POS_WHEEL)wheelIndex, length); +} + +//----------------------------------------------------------------------------- +// Purpose: Allows booster timer to run, +// Returns: true if time still exists +// false if timer has run out (i.e. can use boost again) +//----------------------------------------------------------------------------- +float CVehicleController::UpdateBooster( float dt ) +{ + m_pCarSystem->update_booster( dt ); + m_currentState.boostDelay = m_pCarSystem->get_booster_delay(); + return m_currentState.boostDelay; +} + +//----------------------------------------------------------------------------- +// Purpose: Are whe boosting? +//----------------------------------------------------------------------------- +bool CVehicleController::IsBoosting( void ) +{ + return ( m_pCarSystem->get_booster_time_to_go() > 0.0f ); +} + +//----------------------------------------------------------------------------- +// Purpose: Update the vehicle controller. +//----------------------------------------------------------------------------- +void CVehicleController::Update( float dt, vehicle_controlparams_t &controlsIn ) +{ + vehicle_controlparams_t controls = controlsIn; + // Speed. + m_currentState.speed = ConvertDistanceToHL( m_pCarSystem->get_body_speed() ); + float flSpeed = GAMEVEL_TO_MPH( m_currentState.speed ); + float flAbsSpeed = fabsf( flSpeed ); + + // Calculate the throttle and brake values. + float flThrottle = controls.throttle; + bool bHandbrake = controls.handbrake; + float flBrake = controls.brake; + bool bPowerslide = bHandbrake && ( flAbsSpeed > 18.0f ); + + if ( bHandbrake ) + { + flThrottle = 0.0f; + } + + if ( IsBoosting() ) + { + controls.boost = true; + flThrottle = flThrottle < 0.0f ? -1.0f : 1.0f; + } + + if ( flThrottle == 0.0f && flBrake == 0.0f && !bHandbrake ) + { + flBrake = 0.1f; + } + + // Update steering. + UpdateSteering( controls, dt, flAbsSpeed ); + + // Update powerslide. + UpdatePowerslide( controls, bPowerslide, flSpeed ); + + // Update engine. + UpdateEngine( controls, dt, flThrottle, flBrake, bHandbrake, bPowerslide ); + + // Update handbrake. + UpdateHandbrake( controls, flThrottle, bHandbrake, bPowerslide ); + + // Update skidding. + UpdateSkidding( bHandbrake ); + + // Apply the extra forces to the car (downward, counter-torque, etc.) + UpdateExtraForces(); + + // Update the physical position of the wheels for raycast vehicles. + UpdateWheelPositions(); +} + +//----------------------------------------------------------------------------- +// Purpose: Update the steering on the vehicle. +//----------------------------------------------------------------------------- +void CVehicleController::UpdateSteering( const vehicle_controlparams_t &controls, float flDeltaTime, float flSpeed ) +{ + // Steering - IVP steering is in radians. + float flSteeringAngle = CalcSteering( flDeltaTime, flSpeed, controls.steering, controls.bAnalogSteering ); + m_pCarSystem->do_steering( DEG2RAD( flSteeringAngle ), controls.bAnalogSteering ); + m_currentState.steeringAngle = flSteeringAngle; +} + +//----------------------------------------------------------------------------- +// Purpose: Update the powerslide state (wheel materials). +//----------------------------------------------------------------------------- +void CVehicleController::UpdatePowerslide( const vehicle_controlparams_t &controls, bool bPowerslide, float flSpeed ) +{ + // Only allow skidding if it is allowed by the vehicle type. + if ( !m_vehicleData.steering.isSkidAllowed ) + return; + + // Check to see if the vehicle is occupied. + if ( !m_bOccupied ) + return; + + // Set the powerslide left/right. + bool bPowerslideLeft = bPowerslide && controls.handbrakeLeft; + bool bPowerslideRight = bPowerslide && controls.handbrakeRight; + + int iWheel = 0; + unsigned int newTireType = VEHICLE_TIRE_NORMAL; + if ( bPowerslideLeft || bPowerslideRight ) + { + newTireType = VEHICLE_TIRE_POWERSLIDE; + } + else if ( bPowerslide ) + { + newTireType = VEHICLE_TIRE_BRAKING; + } + + if ( newTireType != m_nTireType ) + { + for ( int iAxle = 0; iAxle < m_vehicleData.axleCount; ++iAxle ) + { + int materialIndex = m_vehicleData.axles[iAxle].wheels.materialIndex; + if ( newTireType == VEHICLE_TIRE_POWERSLIDE && ( m_vehicleData.axles[iAxle].wheels.skidMaterialIndex != - 1 ) ) + { + materialIndex = m_vehicleData.axles[iAxle].wheels.skidMaterialIndex; + } + else if ( newTireType == VEHICLE_TIRE_BRAKING && ( m_vehicleData.axles[iAxle].wheels.brakeMaterialIndex != -1 ) ) + { + materialIndex = m_vehicleData.axles[iAxle].wheels.brakeMaterialIndex; + } + + for ( int iAxleWheel = 0; iAxleWheel < m_vehicleData.wheelsPerAxle; ++iAxleWheel, ++iWheel ) + { + m_pWheels[iWheel]->SetMaterialIndex( materialIndex ); + } + + m_nTireType = newTireType; + } + } + + // Push the car a little. + float flFrontAccel = 0.0f; + float flRearAccel = 0.0f; + if ( flSpeed > 0 && (bPowerslideLeft != bPowerslideRight) ) + { + // NOTE: positive acceleration is to the left + float powerSlide = RemapValClamped( flSpeed, m_vehicleData.steering.speedSlow, m_vehicleData.steering.speedFast, 0, 1 ); + float powerSlideAccel = ConvertDistanceToIVP( m_vehicleData.steering.powerSlideAccel); + if ( bPowerslideLeft ) + { + flFrontAccel = powerSlideAccel * powerSlide; + flRearAccel = -powerSlideAccel * powerSlide; + } + else + { + flFrontAccel = -powerSlideAccel * powerSlide; + flRearAccel = powerSlideAccel * powerSlide; + } + } + m_pCarSystem->set_powerslide( flFrontAccel, flRearAccel ); +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CVehicleController::UpdateEngine( const vehicle_controlparams_t &controls, float flDeltaTime, + float flThrottle, float flBrake, bool bHandbrake, bool bPowerslide ) +{ + bool bTorqueBoost = UpdateEngineTurboStart( controls, flDeltaTime ); + + CalcEngine( flThrottle, flBrake, bHandbrake, controls.steering, bTorqueBoost ); + + UpdateEngineTurboFinish(); +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +bool CVehicleController::UpdateEngineTurboStart( const vehicle_controlparams_t &controls, float flDeltaTime ) +{ + bool bTorqueBoost = false; + if ( controls.boost > 0 ) + { + if ( m_vehicleData.engine.torqueBoost ) + { + // Turbo will be applied at the engine level. + bTorqueBoost = true; + m_pCarSystem->activate_booster( 0.0f, m_vehicleData.engine.boostDuration, m_vehicleData.engine.boostDelay ); + } + else + { + // Activate the turbo force booster - applied to vehicle body. + m_pCarSystem->activate_booster( m_vehicleData.engine.boostForce * controls.boost, m_vehicleData.engine.boostDuration, m_vehicleData.engine.boostDelay ); + } + } + + m_pCarSystem->update_booster( flDeltaTime ); + m_currentState.boostDelay = m_pCarSystem->get_booster_delay(); + m_currentState.isTorqueBoosting = bTorqueBoost; + + return bTorqueBoost; +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CVehicleController::UpdateEngineTurboFinish( void ) +{ + if ( m_vehicleData.engine.boostDuration + m_vehicleData.engine.boostDelay > 0 ) // watch out for div by zero + { + if ( m_currentState.boostDelay > 0 ) + { + m_currentState.boostTimeLeft = 100 - 100 * ( m_currentState.boostDelay / ( m_vehicleData.engine.boostDuration + m_vehicleData.engine.boostDelay ) ); + } + else + { + m_currentState.boostTimeLeft = 100; // ready to go any time + } + } +} + +//----------------------------------------------------------------------------- +// Purpose: Update the handbrake. +//----------------------------------------------------------------------------- +void CVehicleController::UpdateHandbrake( const vehicle_controlparams_t &controls, float flThrottle, bool bHandbrake, bool bPowerslide ) +{ + // Get the current vehicle speed. + m_currentState.speed = ConvertDistanceToHL( m_pCarSystem->get_body_speed() ); + if ( !bPowerslide ) + { + // HACK! Allowing you to overcome gravity at low throttle. + if ( ( flThrottle < 0.0f && m_currentState.speed > THROTTLE_OPPOSING_FORCE_EPSILON ) || + ( flThrottle > 0.0f && m_currentState.speed < -THROTTLE_OPPOSING_FORCE_EPSILON ) ) + { + bHandbrake = true; + } + } + + if ( bHandbrake ) + { + // HACKHACK: only allow the handbrake when the wheels have contact with something + // otherwise they will affect the car in an undesirable way + bHandbrake = false; + for ( int iWheel = 0; iWheel < m_wheelCount; ++iWheel ) + { + if ( m_pWheels[iWheel]->GetContactPoint(NULL, NULL) ) + { + bHandbrake = true; + break; + } + } + } + + bool currentHandbrake = (m_vehicleFlags & FVEHICLE_HANDBRAKE_ON) ? true : false; + if ( bHandbrake != currentHandbrake ) + { + if ( bHandbrake ) + { + m_vehicleFlags |= FVEHICLE_HANDBRAKE_ON; + } + else + { + m_vehicleFlags &= ~FVEHICLE_HANDBRAKE_ON; + } + + for ( int iWheel = 0; iWheel < m_wheelCount; ++iWheel ) + { + m_pCarSystem->fix_wheel( ( IVP_POS_WHEEL )iWheel, bHandbrake ? IVP_TRUE : IVP_FALSE ); + } + } +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CVehicleController::UpdateSkidding( bool bHandbrake ) +{ + m_currentState.skidSpeed = 0.0f; + m_currentState.skidMaterial = 0; + m_currentState.wheelsInContact = m_wheelCount; + m_currentState.wheelsNotInContact = 0; + if ( m_vehicleData.steering.isSkidAllowed ) + { + // Estimate rot speed based on current speed and the front wheels radius + float flAbsSpeed = fabs( m_currentState.speed ); + + Vector contact; + Vector velocity; + int surfaceProps; + m_currentState.wheelsInContact = 0; + m_currentState.wheelsNotInContact = 0; + + for( int iWheel = 0; iWheel < m_wheelCount; ++iWheel ) + { + if ( GetWheelContactPoint( iWheel, &contact, &surfaceProps ) ) + { + // NOTE: The wheel should be translating by the negative of the speed a point in contact with the surface + // is moving. So the net velocity on the surface is zero if that wheel is 100% engaged in driving the car + // any velocity in excess of this gets compared against the threshold for skidding + m_pWheels[iWheel]->GetVelocityAtPoint( contact, &velocity ); + float speed = velocity.Length(); + if ( speed > m_currentState.skidSpeed || m_currentState.skidSpeed <= 0.0f ) + { + m_currentState.skidSpeed = speed; + m_currentState.skidMaterial = surfaceProps; + } + m_currentState.wheelsInContact++; + } + else + { + m_currentState.wheelsNotInContact++; + } + } + // Check for locked wheels. + if ( bHandbrake && ( flAbsSpeed > 30 ) ) + { + m_currentState.skidSpeed = flAbsSpeed; + } + } +} + +//----------------------------------------------------------------------------- +// Purpose: Apply extra forces to the vehicle. The downward force, counter- +// torque etc. +//----------------------------------------------------------------------------- +void CVehicleController::UpdateExtraForces( void ) +{ + // Extra downward force. + IVP_Cache_Object *co = m_pCarBody->GetObject()->get_cache_object(); + float y_val = co->m_world_f_object.get_elem( IVP_INDEX_Y, IVP_INDEX_Y ); + if ( fabs( y_val ) < 0.05 ) + { + m_pCarSystem->change_body_downforce( m_vehicleData.body.tiltForce * m_gravityLength * m_bodyMass ); + } + else + { + m_pCarSystem->change_body_downforce( 0.0 ); + } + co->remove_reference(); + + // Counter-torque. + if ( m_nVehicleType == VEHICLE_TYPE_CAR_WHEELS ) + { + m_pCarSystem->update_body_countertorque(); + } + + // if the car has a global angular velocity limit, apply that constraint + AngularImpulse angVel; + m_pCarBody->GetVelocity( NULL, &angVel ); + if ( m_vehicleData.body.maxAngularVelocity > 0 && angVel.Length() > m_vehicleData.body.maxAngularVelocity ) + { + VectorNormalize(angVel); + angVel *= m_vehicleData.body.maxAngularVelocity; + m_pCarBody->SetVelocityInstantaneous( NULL, &angVel ); + } +} + +//----------------------------------------------------------------------------- +// Purpose: Update the physical position of the wheels for raycast vehicles. +// NOTE: Raycast boat doesn't have wheels. +//----------------------------------------------------------------------------- +void CVehicleController::UpdateWheelPositions( void ) +{ + if ( m_nVehicleType == VEHICLE_TYPE_CAR_RAYCAST ) + { + m_pCarSystem->update_wheel_positions(); + } +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +float CVehicleController::CalcSteering( float dt, float speed, float steering, bool bAnalog ) +{ + float degrees = RemapValClamped( speed, m_vehicleData.steering.speedSlow, m_vehicleData.steering.speedFast, m_vehicleData.steering.degreesSlow, m_vehicleData.steering.degreesFast ); + float speedGame = MPH_TO_GAMEVEL(speed); + if ( speedGame > m_vehicleData.engine.maxSpeed ) + { + degrees = RemapValClamped( speedGame, m_vehicleData.engine.maxSpeed, m_vehicleData.engine.boostMaxSpeed, m_vehicleData.steering.degreesFast, m_vehicleData.steering.degreesBoost ); + } + if ( m_vehicleData.steering.steeringExponent != 0 ) + { + float sign = steering < 0 ? -1 : 1; + float absSteering = fabs(steering); + if ( bAnalog ) + { + // analog steering is directly mapped, not integrated, so go ahead and map the full range using the exponent + // then clamp to the output cone - keeps stick position:turn rate constant + // NOTE: Also hardcode exponent to 2 because we can't add a script entry at this point + float output = pow(absSteering, 2.0f) * sign * m_vehicleData.steering.degreesSlow; + return clamp(output, -degrees, degrees ); + } + // digital steering is integrated, keep time to full turn rate constant + return pow(absSteering, m_vehicleData.steering.steeringExponent) * sign * degrees; + } + return steering * degrees; +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CVehicleController::CalcEngineTransmission( float flThrottle ) +{ + // Automatic Transmission? + if ( !m_vehicleData.engine.isAutoTransmission ) + return; + + // Calculate the average rotational speed of the vehicle's wheels. + float flAvgRotSpeed = 0.0; + for( int iWheel = 0; iWheel < m_wheelCount; ++iWheel ) + { + float flRotSpeed = fabs( m_pCarSystem->get_wheel_angular_velocity( IVP_POS_WHEEL( iWheel ) ) ); + flAvgRotSpeed += flRotSpeed; + } + flAvgRotSpeed *= 0.5f / ( float )IVP_PI / m_wheelCount; + + float flEstEngineRPM = flAvgRotSpeed * m_vehicleData.engine.axleRatio * m_vehicleData.engine.gearRatio[m_currentState.gear] * 60; + + // Only shift up when going forward (throttling). + if ( flThrottle > 0.0f ) + { + // Shift up?, top gear is gearcount-1 (0 based) + while ( ( flEstEngineRPM > m_vehicleData.engine.shiftUpRPM ) && ( m_currentState.gear < m_vehicleData.engine.gearCount-1 ) ) + { + m_currentState.gear++; + flEstEngineRPM = flAvgRotSpeed * m_vehicleData.engine.axleRatio * m_vehicleData.engine.gearRatio[m_currentState.gear] * 60; + } + } + + // Downshift? + while ( ( flEstEngineRPM < m_vehicleData.engine.shiftDownRPM ) && ( m_currentState.gear > 0 ) ) + { + m_currentState.gear--; + flEstEngineRPM = flAvgRotSpeed * m_vehicleData.engine.axleRatio * m_vehicleData.engine.gearRatio[m_currentState.gear] * 60; + } + + m_currentState.engineRPM = flEstEngineRPM; +} + +//----------------------------------------------------------------------------- +// Purpose: +// throttle goes forward and backward, [-1, 1] +// brake_val [0..1] +//----------------------------------------------------------------------------- +void CVehicleController::CalcEngine( float throttle, float brake_val, bool handbrake, float steeringVal, bool torqueBoost ) +{ + // Update the engine transmission. + CalcEngineTransmission( throttle ); + + // Get the speed of the vehicle. + float flAbsSpeed = fabs( m_currentState.speed ); + + // Speed governor + if ( IsPC() ) + { + float maxSpeed = torqueBoost ? m_vehicleData.engine.boostMaxSpeed : m_vehicleData.engine.maxSpeed; + maxSpeed = max(1.f,maxSpeed); // make sure this is non-zero before the divide + if ( (throttle > 0) && (flAbsSpeed > maxSpeed) ) + { + float frac = flAbsSpeed / maxSpeed; + if ( frac > m_vehicleData.engine.autobrakeSpeedGain ) + { + throttle = 0; + brake_val = (frac - 1.0f) * m_vehicleData.engine.autobrakeSpeedFactor; + if ( m_currentState.wheelsInContact == 0 ) + { + brake_val = 0; + } + } + throttle *= 0.1f; + } + } + else // consoles + { + if ( ( throttle > 0 ) && ( ( !torqueBoost && flAbsSpeed > (m_vehicleData.engine.maxSpeed * throttle) ) || + ( torqueBoost && flAbsSpeed > m_vehicleData.engine.boostMaxSpeed) ) ) + { + throttle *= 0.1f; + } + } + + // Check for reverse - both of these "governors" or horrible and need to be redone before we ship! + if ( ( throttle < 0 ) && ( !torqueBoost && ( flAbsSpeed > m_vehicleData.engine.maxRevSpeed ) ) ) + { + throttle *= 0.1f; + } + + if ( throttle != 0.0 ) + { + m_vehicleFlags &= ~FVEHICLE_THROTTLE_STOPPED; + // calculate the force that propels the car + const float watt_per_hp = 745.0f; + const float seconds_per_minute = 60.0f; + + float wheel_force_by_throttle = throttle * + m_vehicleData.engine.horsepower * (watt_per_hp * seconds_per_minute) * + m_vehicleData.engine.gearRatio[m_currentState.gear] * m_vehicleData.engine.axleRatio / + (m_vehicleData.engine.maxRPM * m_wheelRadius * (2 * IVP_PI)); + + if ( m_currentState.engineRPM >= m_vehicleData.engine.maxRPM ) + { + wheel_force_by_throttle = 0; + } + + int wheelIndex = 0; + for ( int i = 0; i < m_vehicleData.axleCount; i++ ) + { + float axleFactor = m_vehicleData.axles[i].torqueFactor * m_torqueScale; + + float boostFactor = 0.5f; + if ( torqueBoost && IsBoosting() ) + { + // reduce the boost at low speeds and high turns since this usually just makes the tires spin + // this means you only get the full boost when travelling in a straight line at high speed + float speedFactor = RemapValClamped( flAbsSpeed, 0, m_vehicleData.engine.maxSpeed, 0.1f, 1.0f ); + float turnFactor = 1.0f - (fabs(steeringVal) * 0.95f); + float dampedBoost = m_vehicleData.engine.boostForce * speedFactor * turnFactor; + if ( dampedBoost > boostFactor ) + { + boostFactor = dampedBoost; + } + //Msg("Boost applied %.2f, speed %.2f, turn %.2f\n", boostFactor, speedFactor, turnFactor ); + } + float axleTorque = boostFactor * wheel_force_by_throttle * axleFactor * ConvertDistanceToIVP( m_vehicleData.axles[i].wheels.radius ); + + for ( int w = 0; w < m_vehicleData.wheelsPerAxle; w++, wheelIndex++ ) + { + float torqueVal = axleTorque; + m_pCarSystem->change_wheel_torque((IVP_POS_WHEEL)wheelIndex, torqueVal); + } + } + } + else if ( brake_val != 0 ) + { + m_vehicleFlags &= ~FVEHICLE_THROTTLE_STOPPED; + + // Brake to slow down the wheel. + float wheel_force_by_brake = brake_val * m_gravityLength * ( m_bodyMass + m_totalWheelMass ); + + float sign = m_currentState.speed >= 0.0f ? -1.0f : 1.0f; + int wheelIndex = 0; + for ( int i = 0; i < m_vehicleData.axleCount; i++ ) + { + float torque_val = 0.5 * sign * wheel_force_by_brake * m_vehicleData.axles[i].brakeFactor * ConvertDistanceToIVP( m_vehicleData.axles[i].wheels.radius ); + for ( int w = 0; w < m_vehicleData.wheelsPerAxle; w++, wheelIndex++ ) + { + m_pCarSystem->change_wheel_torque( ( IVP_POS_WHEEL )wheelIndex, torque_val ); + } + } + } + else if ( !(m_vehicleFlags & FVEHICLE_THROTTLE_STOPPED) ) + { + m_vehicleFlags |= FVEHICLE_THROTTLE_STOPPED; + + for ( int w = 0; w < m_wheelCount; w++ ) + { + m_pCarSystem->change_wheel_torque((IVP_POS_WHEEL)w, 0); + } + } + + // Update the throttle - primarily for the airboat! + m_pCarSystem->update_throttle( throttle ); +} + +//----------------------------------------------------------------------------- +// Purpose: Get debug rendering data from the ipion physics system. +//----------------------------------------------------------------------------- +void CVehicleController::GetCarSystemDebugData( vehicle_debugcarsystem_t &debugCarSystem ) +{ + IVP_CarSystemDebugData_t carSystemDebugData; + memset(&carSystemDebugData,0,sizeof(carSystemDebugData)); + m_pCarSystem->GetCarSystemDebugData( carSystemDebugData ); + + // Raycast car wheel trace data. + for ( int iWheel = 0; iWheel < VEHICLE_DEBUGRENDERDATA_MAX_WHEELS; ++iWheel ) + { + debugCarSystem.vecWheelRaycasts[iWheel][0].x = carSystemDebugData.wheelRaycasts[iWheel][0].k[0]; + debugCarSystem.vecWheelRaycasts[iWheel][0].y = carSystemDebugData.wheelRaycasts[iWheel][0].k[1]; + debugCarSystem.vecWheelRaycasts[iWheel][0].z = carSystemDebugData.wheelRaycasts[iWheel][0].k[2]; + + debugCarSystem.vecWheelRaycasts[iWheel][1].x = carSystemDebugData.wheelRaycasts[iWheel][1].k[0]; + debugCarSystem.vecWheelRaycasts[iWheel][1].y = carSystemDebugData.wheelRaycasts[iWheel][1].k[1]; + debugCarSystem.vecWheelRaycasts[iWheel][1].z = carSystemDebugData.wheelRaycasts[iWheel][1].k[2]; + + debugCarSystem.vecWheelRaycastImpacts[iWheel] = debugCarSystem.vecWheelRaycasts[iWheel][0] + ( carSystemDebugData.wheelRaycastImpacts[iWheel] * + ( debugCarSystem.vecWheelRaycasts[iWheel][1] - debugCarSystem.vecWheelRaycasts[iWheel][0] ) ); + } + + ConvertPositionToHL( carSystemDebugData.backActuatorLeft, debugCarSystem.vecAxlePos[0] ); + ConvertPositionToHL( carSystemDebugData.backActuatorRight, debugCarSystem.vecAxlePos[1] ); + ConvertPositionToHL( carSystemDebugData.frontActuatorLeft, debugCarSystem.vecAxlePos[2] ); + // vecAxlePos only has three elements so this line is illegal. The mapping of actuators + // to axles seems dodgy anyway. + //ConvertPositionToHL( carSystemDebugData.frontActuatorRight, debugCarSystem.vecAxlePos[3] ); +} + + +//----------------------------------------------------------------------------- +// Save/load +//----------------------------------------------------------------------------- +void CVehicleController::WriteToTemplate( vphysics_save_cvehiclecontroller_t &controllerTemplate ) +{ + // Get rid of the handbrake flag. The car keeps the flag and will reset it fixing wheels, + // else the system thinks it already fixed the wheels on load and the car roles. + m_vehicleFlags &= ~FVEHICLE_HANDBRAKE_ON; + + controllerTemplate.m_pCarBody = m_pCarBody; + controllerTemplate.m_wheelCount = m_wheelCount; + controllerTemplate.m_wheelRadius = m_wheelRadius; + controllerTemplate.m_bodyMass = m_bodyMass; + controllerTemplate.m_totalWheelMass = m_totalWheelMass; + controllerTemplate.m_gravityLength = m_gravityLength; + controllerTemplate.m_torqueScale = m_torqueScale; + controllerTemplate.m_vehicleFlags = m_vehicleFlags; + controllerTemplate.m_nTireType = m_nTireType; + controllerTemplate.m_nVehicleType = m_nVehicleType; + controllerTemplate.m_bTraceData = m_bTraceData; + controllerTemplate.m_bOccupied = m_bOccupied; + controllerTemplate.m_bEngineDisable = m_bEngineDisable; + memcpy( &controllerTemplate.m_currentState, &m_currentState, sizeof(m_currentState) ); + memcpy( &controllerTemplate.m_vehicleData, &m_vehicleData, sizeof(m_vehicleData) ); + for (int i = 0; i < VEHICLE_MAX_WHEEL_COUNT; ++i ) + { + controllerTemplate.m_pWheels[i] = m_pWheels[i]; + ConvertPositionToHL( m_wheelPosition_Bs[i], controllerTemplate.m_wheelPosition_Bs[i] ); + ConvertPositionToHL( m_tracePosition_Bs[i], controllerTemplate.m_tracePosition_Bs[i] ); + } + m_flVelocity[0] = m_flVelocity[1] = m_flVelocity[2] = 0.0f; + if ( m_pCarBody ) + { + IVP_U_Float_Point &speed = m_pCarBody->GetObject()->get_core()->speed; + controllerTemplate.m_flVelocity[0] = speed.k[0]; + controllerTemplate.m_flVelocity[1] = speed.k[1]; + controllerTemplate.m_flVelocity[2] = speed.k[2]; + } + +} + +// JAY: Keep this around for now while we still have a bunch of games saved with the old +// vehicle controls. We won't ship this, but it lets us debug +#define OLD_SAVED_GAME 1 + +#if OLD_SAVED_GAME +#define SET_DEFAULT(x,y) { if ( x == 0 ) x = y; } +#endif + +void CVehicleController::InitFromTemplate( CPhysicsEnvironment *pEnv, void *pGameData, + IPhysicsGameTrace *pGameTrace, const vphysics_save_cvehiclecontroller_t &controllerTemplate ) +{ + m_pEnv = pEnv; + m_pGameTrace = pGameTrace; + m_pCarBody = controllerTemplate.m_pCarBody; + m_wheelCount = controllerTemplate.m_wheelCount; + m_wheelRadius = controllerTemplate.m_wheelRadius; + m_bodyMass = controllerTemplate.m_bodyMass; + m_totalWheelMass = controllerTemplate.m_totalWheelMass; + m_gravityLength = controllerTemplate.m_gravityLength; + m_torqueScale = controllerTemplate.m_torqueScale; + m_vehicleFlags = controllerTemplate.m_vehicleFlags; + m_nTireType = controllerTemplate.m_nTireType; + m_nVehicleType = controllerTemplate.m_nVehicleType; + m_bTraceData = controllerTemplate.m_bTraceData; + m_bOccupied = controllerTemplate.m_bOccupied; + m_bEngineDisable = controllerTemplate.m_bEngineDisable; + m_pCarSystem = NULL; + memcpy( &m_currentState, &controllerTemplate.m_currentState, sizeof(m_currentState) ); + memcpy( &m_vehicleData, &controllerTemplate.m_vehicleData, sizeof(m_vehicleData) ); + memcpy( &m_flVelocity, controllerTemplate.m_flVelocity, sizeof(m_flVelocity) ); + +#if OLD_SAVED_GAME + SET_DEFAULT( m_torqueScale, 1.0 ); + SET_DEFAULT( m_vehicleData.steering.steeringRateSlow, 4.5 ); + SET_DEFAULT( m_vehicleData.steering.steeringRateFast, 0.5 ); + SET_DEFAULT( m_vehicleData.steering.steeringRestRateSlow, 3.0 ); + SET_DEFAULT( m_vehicleData.steering.steeringRestRateFast, 1.8 ); + SET_DEFAULT( m_vehicleData.steering.speedSlow, m_vehicleData.engine.maxSpeed*0.25 ); + SET_DEFAULT( m_vehicleData.steering.speedFast, m_vehicleData.engine.maxSpeed*0.75 ); + SET_DEFAULT( m_vehicleData.steering.degreesSlow, 50 ); + SET_DEFAULT( m_vehicleData.steering.degreesFast, 18 ); + SET_DEFAULT( m_vehicleData.steering.degreesBoost, 10 ); + + + SET_DEFAULT( m_vehicleData.steering.turnThrottleReduceSlow, 0.3 ); + SET_DEFAULT( m_vehicleData.steering.turnThrottleReduceFast, 3 ); + SET_DEFAULT( m_vehicleData.steering.brakeSteeringRateFactor, 6 ); + SET_DEFAULT( m_vehicleData.steering.throttleSteeringRestRateFactor, 2 ); + SET_DEFAULT( m_vehicleData.steering.boostSteeringRestRateFactor, 1 ); + SET_DEFAULT( m_vehicleData.steering.boostSteeringRateFactor, 1 ); + SET_DEFAULT( m_vehicleData.steering.powerSlideAccel, 200 ); + + SET_DEFAULT( m_vehicleData.engine.autobrakeSpeedGain, 1.0 ); + SET_DEFAULT( m_vehicleData.engine.autobrakeSpeedFactor, 2.0 ); +#endif + + for (int i = 0; i < VEHICLE_MAX_WHEEL_COUNT; ++i ) + { + m_pWheels[i] = controllerTemplate.m_pWheels[i]; + ConvertPositionToIVP( controllerTemplate.m_wheelPosition_Bs[i], m_wheelPosition_Bs[i] ); + ConvertPositionToIVP( controllerTemplate.m_tracePosition_Bs[i], m_tracePosition_Bs[i] ); + } + + CreateIVPObjects( ); + + // HACKHACK: vehicle wheels don't have valid friction at startup, clearing the body's angular velocity keeps + // this fact from affecting the vehicle dynamics in any noticeable way + // using growFriction will re-establish the contact point with moveable objects, but the friction that + // occurs afterward is not the same across the save even when that is extended to include static objects + if ( m_pCarBody ) + { + // clear angVel + m_pCarBody->SetVelocity( NULL, &vec3_origin ); + m_pCarBody->GetObject()->get_core()->speed_change.set( m_flVelocity[0], m_flVelocity[1], m_flVelocity[2] ); + } +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CVehicleController::OnVehicleEnter( void ) +{ + m_bOccupied = true; + + if ( m_nVehicleType == VEHICLE_TYPE_AIRBOAT_RAYCAST ) + { + float flDampSpeed = 0.0f; + float flDampRotSpeed = 0.0f; + m_pCarBody->SetDamping( &flDampSpeed, &flDampRotSpeed ); + } +} + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +void CVehicleController::OnVehicleExit( void ) +{ + m_bOccupied = false; + + // Reset the vehicle tires when exiting the vehicle. + if ( m_vehicleData.steering.isSkidAllowed ) + { + int iWheel = 0; + for ( int iAxle = 0; iAxle < m_vehicleData.axleCount; ++iAxle ) + { + for ( int iAxleWheel = 0; iAxleWheel < m_vehicleData.wheelsPerAxle; ++iAxleWheel, ++iWheel ) + { + // Change back to normal tires. + if ( m_nTireType != VEHICLE_TIRE_NORMAL ) + { + m_pWheels[iWheel]->SetMaterialIndex( m_vehicleData.axles[iAxle].wheels.materialIndex ); + } + + m_pCarSystem->fix_wheel( ( IVP_POS_WHEEL )iWheel, IVP_TRUE ); + } + } + m_nTireType = VEHICLE_TIRE_NORMAL; + m_currentState.skidSpeed = 0.0f; + } + + if ( m_nVehicleType == VEHICLE_TYPE_AIRBOAT_RAYCAST ) + { + float flDampSpeed = 1.0f; + float flDampRotSpeed = 1.0f; + m_pCarBody->SetDamping( &flDampSpeed, &flDampRotSpeed ); + } + + SetEngineDisabled( false ); +} + +//----------------------------------------------------------------------------- +// Class factory +//----------------------------------------------------------------------------- +IPhysicsVehicleController *CreateVehicleController( CPhysicsEnvironment *pEnv, CPhysicsObject *pBodyObject, const vehicleparams_t ¶ms, unsigned int nVehicleType, IPhysicsGameTrace *pGameTrace ) +{ + CVehicleController *pController = new CVehicleController( params, pEnv, nVehicleType, pGameTrace ); + pController->InitCarSystem( pBodyObject ); + return pController; +} + +bool SavePhysicsVehicleController( const physsaveparams_t ¶ms, CVehicleController *pVehicleController ) +{ + vphysics_save_cvehiclecontroller_t controllerTemplate; + memset( &controllerTemplate, 0, sizeof(controllerTemplate) ); + + pVehicleController->WriteToTemplate( controllerTemplate ); + params.pSave->WriteAll( &controllerTemplate ); + + return true; +} + +bool RestorePhysicsVehicleController( const physrestoreparams_t ¶ms, CVehicleController **ppVehicleController ) +{ + *ppVehicleController = new CVehicleController; + + vphysics_save_cvehiclecontroller_t controllerTemplate; + memset( &controllerTemplate, 0, sizeof(controllerTemplate) ); + params.pRestore->ReadAll( &controllerTemplate ); + + (*ppVehicleController)->InitFromTemplate( static_cast<CPhysicsEnvironment *>(params.pEnvironment), + params.pGameData, params.pGameTrace, controllerTemplate ); + + return true; +} + + diff --git a/vphysics/physics_vehicle.h b/vphysics/physics_vehicle.h new file mode 100644 index 0000000..c22f377 --- /dev/null +++ b/vphysics/physics_vehicle.h @@ -0,0 +1,25 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef PHYSICS_VEHICLE_H +#define PHYSICS_VEHICLE_H +#ifdef _WIN32 +#pragma once +#endif + + +struct vehicleparams_t; +class IPhysicsVehicleController; +class CPhysicsObject; +class CPhysicsEnvironment; +class IVP_Real_Object; + +bool ShouldOverrideWheelContactFriction( float *pFrictionOut, IVP_Real_Object *pivp0, IVP_Real_Object *pivp1, IVP_U_Float_Point *pNormal ); + +IPhysicsVehicleController *CreateVehicleController( CPhysicsEnvironment *pEnv, CPhysicsObject *pBodyObject, const vehicleparams_t ¶ms, unsigned int nVehicleType, IPhysicsGameTrace *pGameTrace ); + +#endif // PHYSICS_VEHICLE_H diff --git a/vphysics/physics_virtualmesh.cpp b/vphysics/physics_virtualmesh.cpp new file mode 100644 index 0000000..fbd67b9 --- /dev/null +++ b/vphysics/physics_virtualmesh.cpp @@ -0,0 +1,639 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: Virtual mesh implementation. Cached terrain collision model +// +//============================================================================= + + +#include "cbase.h" +#include "convert.h" +#include "ivp_surface_manager.hxx" +#include "ivp_surman_polygon.hxx" +#include "ivp_template_surbuild.hxx" +#include "ivp_compact_surface.hxx" +#include <ivp_compact_ledge.hxx> +#include <ivp_ray_solver.hxx> +#include <ivp_compact_ledge_solver.hxx> +#include "ivp_surbuild_pointsoup.hxx" +#include "ivp_surbuild_ledge_soup.hxx" +#include "physics_trace.h" +#include "collisionutils.h" +#include "datamanager.h" +#include "utlbuffer.h" +#include "ledgewriter.h" +#include "tier1/mempool.h" +#include "tier0/memdbgon.h" + +class CPhysCollideVirtualMesh; + +CTSPool< CUtlVector<CPhysCollideVirtualMesh *> > g_MeshFrameLocksPool; +CThreadLocalPtr< CUtlVector<CPhysCollideVirtualMesh *> > g_pMeshFrameLocks; + +// This is the surfacemanager class for IVP that implements the required functions by layering CPhysCollideVirtualMesh +class IVP_SurfaceManager_VirtualMesh : public IVP_SurfaceManager +{ +public: + void add_reference_to_ledge(const IVP_Compact_Ledge *ledge); + void remove_reference_to_ledge(const IVP_Compact_Ledge *ledge); + void insert_all_ledges_hitting_ray(IVP_Ray_Solver *ray_solver, IVP_Real_Object *object); + void get_radius_and_radius_dev_to_given_center(const IVP_U_Float_Point *center, IVP_FLOAT *radius, IVP_FLOAT *radius_deviation) const; + virtual IVP_SURMAN_TYPE get_type() { return IVP_SURMAN_POLYGON; } + + // assume mesh is never a single triangle + virtual const IVP_Compact_Ledge *get_single_convex() const; + void get_mass_center(IVP_U_Float_Point *mass_center_out) const; + void get_rotation_inertia( IVP_U_Float_Point *rotation_inertia_out ) const; + void get_all_ledges_within_radius(const IVP_U_Point *observer_os, IVP_DOUBLE radius, + const IVP_Compact_Ledge *root_ledge, IVP_Real_Object *other_object, const IVP_Compact_Ledge *other_reference_ledge, + IVP_U_BigVector<IVP_Compact_Ledge> *resulting_ledges); + + void get_all_terminal_ledges(IVP_U_BigVector<IVP_Compact_Ledge> *resulting_ledges); + IVP_SurfaceManager_VirtualMesh( CPhysCollideVirtualMesh *pMesh ); + virtual ~IVP_SurfaceManager_VirtualMesh(); + +private: + CPhysCollideVirtualMesh *m_pMesh; +}; + +// These are the managed objects for the LRU of terrain collisions +// These get created/destroyed dynamically by a resourcemanager +// These contain the uncompressed collision models for each displacement patch +// The idea is to have only the necessary instances of these in memory at any given time - never all of them +class CMeshInstance +{ +public: + // resourcemanager + static unsigned int EstimatedSize( const virtualmeshlist_t &list ); + static CMeshInstance *CreateResource( const virtualmeshlist_t &list ); + static unsigned int ComputeRootLedgeSize( const byte *pHull ); + void DestroyResource() { delete this; } + unsigned int Size() { return m_memSize; } + CMeshInstance *GetData() { return this; } + const triangleledge_t *GetLedges() { return (triangleledge_t *)m_pMemory; } + inline int HullCount() { return m_hullCount; } + const IVP_Compact_Ledge *GetOuterHull() { return (m_hullCount==1) ? (const IVP_Compact_Ledge *)(m_pMemory + m_hullOffset) : NULL; } + int GetRootLedges( IVP_Compact_Ledge **pLedges, int outCount ) + { + int hullOffset = m_hullOffset; + int count = min(outCount, (int)m_hullCount); + for ( int i = 0; i < count; i++ ) + { + pLedges[i] = (IVP_Compact_Ledge *)(m_pMemory + hullOffset); + hullOffset += sizeof(IVP_Compact_Ledge) + (sizeof(IVP_Compact_Triangle) * pLedges[i]->get_n_triangles()); + } + return count; + } + + // locals + CMeshInstance() { m_pMemory = 0; } + ~CMeshInstance(); + +private: + void Init( const virtualmeshlist_t &list ); + + int m_memSize; + char *m_pMemory; + unsigned short m_hullOffset; + byte m_hullCount; + byte m_pad; +}; + +CMeshInstance::~CMeshInstance() +{ + if ( m_pMemory ) + { + ivp_free_aligned( m_pMemory ); + m_pMemory = NULL; + } +} + +unsigned int CMeshInstance::EstimatedSize( const virtualmeshlist_t &list ) +{ + int ledgeSize = sizeof(triangleledge_t) * list.triangleCount; + int pointSize = sizeof(IVP_Compact_Poly_Point) * list.vertexCount; + + int hullSize = ComputeRootLedgeSize(list.pHull); + return ledgeSize + pointSize + hullSize; +} + +// computes the unpacked size of the array of root ledges +unsigned int CMeshInstance::ComputeRootLedgeSize( const byte *pData ) +{ + if ( !pData ) + return 0; + virtualmeshhull_t *pHeader = (virtualmeshhull_t *)pData; + packedhull_t *pHull = (packedhull_t *)(pHeader+1); + unsigned int size = pHeader->hullCount * sizeof(IVP_Compact_Ledge); + for ( int i = 0; i < pHeader->hullCount; i++ ) + { + size += sizeof(IVP_Compact_Triangle) * pHull[i].triangleCount; + } + return size; +} + +CMeshInstance *CMeshInstance::CreateResource( const virtualmeshlist_t &list ) +{ + CMeshInstance *pMesh = new CMeshInstance; + pMesh->Init( list ); + return pMesh; +} + + +// flat memory footprint has triangleledges (ledge + 2 triangles for terrain), then has verts, then optional convex hull +void CMeshInstance::Init( const virtualmeshlist_t &list ) +{ + int ledgeSize = sizeof(triangleledge_t) * list.triangleCount; + int pointSize = sizeof(IVP_Compact_Poly_Point) * list.vertexCount; + int memSize = ledgeSize + pointSize + ComputeRootLedgeSize(list.pHull); + m_memSize = memSize; + m_hullCount = 0; + m_pMemory = (char *)ivp_malloc_aligned( memSize, 16 ); + Assert( (int(m_pMemory) & 15) == 0 ); // make sure it is aligned + IVP_Compact_Poly_Point *pPoints = (IVP_Compact_Poly_Point *)&m_pMemory[ledgeSize]; + triangleledge_t *pLedges = (triangleledge_t *) m_pMemory; + memset( m_pMemory, 0, memSize ); + int i; + + for ( i = 0; i < list.vertexCount; i++ ) + { + ConvertPositionToIVP( list.pVerts[i], pPoints[i] ); + } + + for ( i = 0; i < list.triangleCount; i++ ) + { + Vector v0 = list.pVerts[list.indices[i*3+0]]; + Vector v1 = list.pVerts[list.indices[i*3+1]]; + Vector v2 = list.pVerts[list.indices[i*3+2]]; + Assert( v0 != v1 && v1 != v2 && v0 != v2 ); + CVPhysicsVirtualMeshWriter::InitTwoSidedTriangleLege( &pLedges[i], pPoints, list.indices[i*3+0], list.indices[i*3+1], list.indices[i*3+2], 0 ); + } + Assert( list.triangleCount > 0 && list.triangleCount <= MAX_VIRTUAL_TRIANGLES ); + // if there's a hull, build it out too + if ( list.pHull ) + { + virtualmeshhull_t *pHeader = (virtualmeshhull_t *)list.pHull; + m_hullCount = pHeader->hullCount; + Assert( (ledgeSize + pointSize) < 65536 ); + m_hullOffset = ledgeSize + pointSize; + byte *pMem = (byte *)m_pMemory + m_hullOffset; +#if _DEBUG + int hullSize = CVPhysicsVirtualMeshWriter::UnpackLedgeListFromHull( pMem, pHeader, pPoints ); + Assert((m_hullOffset+hullSize)==memSize); +#else + CVPhysicsVirtualMeshWriter::UnpackLedgeListFromHull( pMem, pHeader, pPoints ); +#endif + } +} + +// UNDONE: Tune / expose this constant 512K budget for terrain collision +const int g_MeshSize = (2048 * 1024); +static CDataManager<CMeshInstance, virtualmeshlist_t, CMeshInstance *, CThreadFastMutex> g_MeshManager( g_MeshSize ); +static int numIndices = 0, numTriangles = 0, numBaseTriangles = 0, numSplits = 0; +//----------------------------------------------------------------------------- +// Purpose: This allows for just-in-time procedural triangle soup data to be +// instanced & cached as IVP collision data (compact ledges) +//----------------------------------------------------------------------------- +// NOTE: This is the permanent in-memory representation. It holds the compressed data +// and the parameters necessary to request the proxy geometry as needed +class CPhysCollideVirtualMesh : public CPhysCollide +{ +public: + // UNDONE: Unlike other CPhysCollide objects, operations the virtual mesh are + // non-const because they may instantiate the cache. This causes problems with the interface. + // Maybe the cache stuff should be mutable, but it amounts to the same kind of + // hackery to cast away const. + + // get a surface manager + virtual IVP_SurfaceManager *CreateSurfaceManager( short &collideType ) const + { + collideType = COLLIDE_VIRTUAL; + // UNDONE: Figure out how to avoid this const_cast + return new IVP_SurfaceManager_VirtualMesh(const_cast<CPhysCollideVirtualMesh *>(this)); + } + virtual void GetAllLedges( IVP_U_BigVector<IVP_Compact_Ledge> &ledges ) const + { + const triangleledge_t *pLedges = const_cast<CPhysCollideVirtualMesh *>(this)->AddRef()->GetLedges(); + for ( int i = 0; i < m_ledgeCount; i++ ) + { + ledges.add( const_cast<IVP_Compact_Ledge *>(&pLedges[i].ledge) ); + } + const_cast<CPhysCollideVirtualMesh *>(this)->Release(); + } + virtual unsigned int GetSerializationSize() const + { + if ( !m_pHull ) + return 0; + return m_pHull->TotalSize(); + } + + virtual unsigned int SerializeToBuffer( char *pDest, bool bSwap = false ) const + { + unsigned int size = GetSerializationSize(); + if ( size ) + { + memcpy( pDest, m_pHull, size ); + } + return size; + } + virtual int GetVCollideIndex() const { return 0; } + virtual void SetMassCenter( const Vector &massCenter ) {Assert(0); } + virtual Vector GetOrthographicAreas() const { return Vector(1,1,1);} + + Vector GetMassCenter() const; + virtual float GetSphereRadius() const; + float GetSphereRadiusIVP() const; + void Init( const char *pBuffer, unsigned int size ) + { + } + void GetAllLedgesWithinRadius( const IVP_U_Point *observer_os, IVP_DOUBLE radius, IVP_U_BigVector<IVP_Compact_Ledge> *resulting_ledges, const IVP_Compact_Ledge *pRootLedge = NULL ) + { + virtualmeshtrianglelist_t list; + list.triangleCount = 0; + Vector centerHL; + ConvertPositionToHL( *observer_os, centerHL ); + float radiusHL = ConvertDistanceToHL(radius); + m_params.pMeshEventHandler->GetTrianglesInSphere( m_params.userData, centerHL, radiusHL, &list ); + if ( list.triangleCount ) + { + CMeshInstance *pMesh = AddRef(); + const triangleledge_t *pLedges = pMesh->GetLedges(); + FrameRelease(); + + // If we have two root ledges, then each one contains half the triangles + // only return triangles indexed under the root ledge being queried + int minTriangle = 0; + int maxTriangle = m_ledgeCount; + if ( pMesh->HullCount() > 1 ) + { + Assert(pMesh->HullCount()==2); + IVP_Compact_Ledge *pRootNodes[2]; + pMesh->GetRootLedges( pRootNodes, 2 ); + int midTriangle = m_ledgeCount/2; + if ( pRootLedge == pRootNodes[0] ) + { + maxTriangle = midTriangle; + } + else + { + minTriangle = midTriangle; + } + } + IVP_DOUBLE radiusSq = radius * radius; + for ( int i = 0; i < list.triangleCount; i++ ) + { + Assert( list.triangleIndices[i] < m_ledgeCount ); + if ( list.triangleIndices[i] < minTriangle || list.triangleIndices[i] >= maxTriangle ) + continue; + + const IVP_Compact_Ledge *ledge = &pLedges[list.triangleIndices[i]].ledge; + Assert(ledge->get_n_triangles() == 2); + const IVP_Compact_Triangle *triangle = ledge->get_first_triangle(); + IVP_DOUBLE qdist = IVP_CLS.calc_qlen_PF_F_space(ledge, triangle, observer_os); + if ( qdist > radiusSq ) + { + continue; + } + + resulting_ledges->add( const_cast<IVP_Compact_Ledge *>(ledge) ); + } + } + } + + virtual void OutputDebugInfo() const + { + Msg("Virtual mesh!\n"); + } + + CPhysCollideVirtualMesh(const virtualmeshparams_t ¶ms) : m_params(params), m_hMemory( INVALID_MEMHANDLE ), m_ledgeCount( 0 ) + { + m_pHull = NULL; + if ( params.buildOuterHull ) + { + BuildBoundingLedge(); + } + } + + virtual ~CPhysCollideVirtualMesh(); + + // adds a lock on the collsion memory :: MUST CALL Release() or FrameRelease corresponding to this call!!! + CMeshInstance *AddRef(); + + void BuildBoundingLedge(); + static virtualmeshhull_t *CreateMeshBoundingHull( const virtualmeshlist_t &list ); + static void DestroyMeshBoundingHull(virtualmeshhull_t *pHull) { CVPhysicsVirtualMeshWriter::DestroyPackedHull(pHull); } + static IVP_Compact_Surface *CreateBoundingSurfaceFromRange( const virtualmeshlist_t &list, int firstIndex, int indexCount ); + + int GetRootLedges( IVP_Compact_Ledge **pLedges, int outCount ) + { + int count = AddRef()->GetRootLedges(pLedges, outCount); + FrameRelease(); + return count; + } + + IVP_Compact_Ledge *GetBoundingLedge() + { + IVP_Compact_Ledge *pLedge = const_cast<IVP_Compact_Ledge *>(AddRef()->GetOuterHull()); + FrameRelease(); + return pLedge; + } + + // releases a lock on the collision memory + void Release(); + // Analagous to Release, but happens at the end of the frame + void FrameRelease() + { + CUtlVector<CPhysCollideVirtualMesh *> *pLocks = g_pMeshFrameLocks; + if ( !pLocks ) + { + g_pMeshFrameLocks = pLocks = g_MeshFrameLocksPool.GetObject(); + Assert( pLocks ); + } + pLocks->AddToTail(this); + } + inline void GetBounds( Vector &mins, Vector &maxs ) const + { + m_params.pMeshEventHandler->GetWorldspaceBounds( m_params.userData, &mins, &maxs ); + } + +private: + CMeshInstance *BuildLedges(); + + virtualmeshparams_t m_params; + virtualmeshhull_t *m_pHull; + memhandle_t m_hMemory; + short m_ledgeCount; +}; + +static void FlushFrameLocks() +{ + CUtlVector<CPhysCollideVirtualMesh *> *pLocks = g_pMeshFrameLocks; + if ( pLocks ) + { + for ( int i = 0; i < pLocks->Count(); i++ ) + { + Assert( (*pLocks)[i] ); + (*pLocks)[i]->Release(); + } + pLocks->RemoveAll(); + g_MeshFrameLocksPool.PutObject( g_pMeshFrameLocks ); + g_pMeshFrameLocks = NULL; + } +} + +void VirtualMeshPSI() +{ + FlushFrameLocks(); +} + + +Vector CPhysCollideVirtualMesh::GetMassCenter() const +{ + Vector mins, maxs; + GetBounds( mins, maxs ); + return 0.5 * (mins + maxs); +} + +float CPhysCollideVirtualMesh::GetSphereRadius() const +{ + Vector mins, maxs; + GetBounds( mins, maxs ); + Vector point = 0.5 * (mins+maxs); + return (maxs - point).Length(); +} + +float CPhysCollideVirtualMesh::GetSphereRadiusIVP() const +{ + return ConvertDistanceToIVP( GetSphereRadius() ); +} + +static CThreadFastMutex s_BuildVirtualMeshMutex; +CMeshInstance *CPhysCollideVirtualMesh::AddRef() +{ + CMeshInstance *pMesh = g_MeshManager.LockResource( m_hMemory ); + if ( !pMesh ) + { + s_BuildVirtualMeshMutex.Lock(); + pMesh = g_MeshManager.LockResource( m_hMemory ); + if ( !pMesh ) + { + pMesh = BuildLedges(); + } + s_BuildVirtualMeshMutex.Unlock(); + } + Assert( pMesh ); + return pMesh; +} + +void CPhysCollideVirtualMesh::Release() +{ + g_MeshManager.UnlockResource( m_hMemory ); +} + +CPhysCollideVirtualMesh::~CPhysCollideVirtualMesh() +{ + CVPhysicsVirtualMeshWriter::DestroyPackedHull(m_pHull); + g_MeshManager.DestroyResource( m_hMemory ); +} + +CMeshInstance *CPhysCollideVirtualMesh::BuildLedges() +{ + virtualmeshlist_t list; + m_params.pMeshEventHandler->GetVirtualMesh( m_params.userData, &list ); + if ( !list.pHull ) + { + list.pHull = (byte *)m_pHull; + } + + if ( list.triangleCount ) + { + m_hMemory = g_MeshManager.CreateResource( list ); + m_ledgeCount = list.triangleCount; + CMeshInstance *pMesh = g_MeshManager.LockResource( m_hMemory ); + return pMesh; + } + return NULL; +} + +// build the outer ledge, split into two if necessary +void CPhysCollideVirtualMesh::BuildBoundingLedge() +{ + virtualmeshlist_t list; + m_params.pMeshEventHandler->GetVirtualMesh( m_params.userData, &list ); + m_pHull = CreateMeshBoundingHull(list); +} + +virtualmeshhull_t *CPhysCollideVirtualMesh::CreateMeshBoundingHull( const virtualmeshlist_t &list ) +{ + virtualmeshhull_t *pHull = NULL; + if ( list.triangleCount ) + { + IVP_Compact_Surface *pSurface = CreateBoundingSurfaceFromRange( list, 0, list.indexCount ); + if ( pSurface ) + { + const IVP_Compact_Ledge *pLedge = pSurface->get_compact_ledge_tree_root()->get_compact_hull(); + if ( CVPhysicsVirtualMeshWriter::LedgeCanBePacked(pLedge, list) ) + { + pHull = CVPhysicsVirtualMeshWriter::CreatePackedHullFromLedges( list, &pLedge, 1 ); + } + else + { + // too big to pack to 8-bits, split in two + IVP_Compact_Surface *pSurface0 = CreateBoundingSurfaceFromRange( list, 0, list.indexCount/2 ); + IVP_Compact_Surface *pSurface1 = CreateBoundingSurfaceFromRange( list, list.indexCount/2, list.indexCount/2 ); + + const IVP_Compact_Ledge *pLedges[2] = {pSurface0->get_compact_ledge_tree_root()->get_compact_hull(), pSurface1->get_compact_ledge_tree_root()->get_compact_hull()}; + pHull = CVPhysicsVirtualMeshWriter::CreatePackedHullFromLedges( list, pLedges, 2 ); + ivp_free_aligned(pSurface0); + ivp_free_aligned(pSurface1); + } + ivp_free_aligned(pSurface); + } + } + return pHull; +} + +IVP_Compact_Surface *CPhysCollideVirtualMesh::CreateBoundingSurfaceFromRange( const virtualmeshlist_t &list, int firstIndex, int indexCount ) +{ + Assert( list.triangleCount ); + IVP_U_Point triVerts[3]; + IVP_U_Vector<IVP_U_Point> triList; + IVP_SurfaceBuilder_Ledge_Soup builder; + triList.add( &triVerts[0] ); + triList.add( &triVerts[1] ); + triList.add( &triVerts[2] ); + int lastIndex = firstIndex + indexCount; + int firstTriangle = firstIndex/3; + int lastTriangle = lastIndex/3; + for ( int i = firstTriangle; i < lastTriangle; i++ ) + { + ConvertPositionToIVP( list.pVerts[list.indices[i*3+0]], triVerts[0] ); + ConvertPositionToIVP( list.pVerts[list.indices[i*3+1]], triVerts[1] ); + ConvertPositionToIVP( list.pVerts[list.indices[i*3+2]], triVerts[2] ); + IVP_Compact_Ledge *pLedge = IVP_SurfaceBuilder_Pointsoup::convert_pointsoup_to_compact_ledge( &triList ); + builder.insert_ledge( pLedge ); + } + // build a convex hull of those verts + IVP_Template_Surbuild_LedgeSoup params; + params.build_root_convex_hull = IVP_TRUE; + IVP_Compact_Surface *pSurface = builder.compile( ¶ms ); + +#if _DEBUG + const IVP_Compact_Ledgetree_Node *node = pSurface->get_compact_ledge_tree_root(); + IVP_Compact_Ledge *pLedge = const_cast<IVP_Compact_Ledge *>(node->get_compact_hull()); // we're going to write into client data on each vert before we throw this away + Assert(pLedge && !pLedge->is_terminal()); +#endif + return pSurface; +} + +CPhysCollide *CreateVirtualMesh( const virtualmeshparams_t ¶ms ) +{ + return new CPhysCollideVirtualMesh(params); +} + +void DestroyVirtualMesh( CPhysCollide *pMesh ) +{ + FlushFrameLocks(); + delete pMesh; +} + +//----------------------------------------------------------------------------- +// IVP_SurfaceManager_VirtualMesh +// This hooks the underlying collision model to IVP's surfacemanager interface +//----------------------------------------------------------------------------- + +IVP_SurfaceManager_VirtualMesh::IVP_SurfaceManager_VirtualMesh( CPhysCollideVirtualMesh *pMesh ) : m_pMesh(pMesh) +{ +} + +IVP_SurfaceManager_VirtualMesh::~IVP_SurfaceManager_VirtualMesh() +{ +} + +void IVP_SurfaceManager_VirtualMesh::add_reference_to_ledge(const IVP_Compact_Ledge *ledge) +{ + m_pMesh->AddRef(); +} +void IVP_SurfaceManager_VirtualMesh::remove_reference_to_ledge(const IVP_Compact_Ledge *ledge) +{ + m_pMesh->Release(); +} + +// Implement the IVP raycast. This is done by testing each triangle (front & back) - so it's slow +void IVP_SurfaceManager_VirtualMesh::insert_all_ledges_hitting_ray(IVP_Ray_Solver *ray_solver, IVP_Real_Object *object) +{ + IVP_Vector_of_Ledges_256 ledges; + IVP_Ray_Solver_Os ray_solver_os( ray_solver, object); + + IVP_U_Point center(&ray_solver_os.ray_center_point); + m_pMesh->GetAllLedgesWithinRadius( ¢er, ray_solver_os.ray_length * 0.5f, &ledges ); + + for (int i=ledges.len()-1;i>=0;i--) + { + const IVP_Compact_Ledge *l = ledges.element_at(i); + ray_solver_os.check_ray_against_compact_ledge_os(l); + } +} + +// Used to predict collision detection needs +void IVP_SurfaceManager_VirtualMesh::get_radius_and_radius_dev_to_given_center(const IVP_U_Float_Point *center, IVP_FLOAT *radius, IVP_FLOAT *radius_deviation) const +{ + // UNDONE: Check radius_deviation to see if there is a useful optimization to be made here + *radius = m_pMesh->GetSphereRadiusIVP(); + *radius_deviation = *radius; +} + +// get a single convex if appropriate +const IVP_Compact_Ledge *IVP_SurfaceManager_VirtualMesh::get_single_convex() const +{ + return m_pMesh->GetBoundingLedge(); +} + +// get a mass center for objects using this collision rep +void IVP_SurfaceManager_VirtualMesh::get_mass_center(IVP_U_Float_Point *mass_center_out) const +{ + Vector center = m_pMesh->GetMassCenter(); + ConvertPositionToIVP( center, *mass_center_out ); +} + +//----------------------------------------------------------------------------- +// Purpose: Compute a diagonalized inertia tensor. +//----------------------------------------------------------------------------- +void IVP_SurfaceManager_VirtualMesh::get_rotation_inertia( IVP_U_Float_Point *rotation_inertia_out ) const +{ + // HACKHACK: No need for this because we only support static objects for now + rotation_inertia_out->set(1,1,1); +} + +//----------------------------------------------------------------------------- +// Purpose: Query ledges (triangles in this case) in sphere +//----------------------------------------------------------------------------- +void IVP_SurfaceManager_VirtualMesh::get_all_ledges_within_radius(const IVP_U_Point *observer_os, IVP_DOUBLE radius, + const IVP_Compact_Ledge *root_ledge, IVP_Real_Object *other_object, const IVP_Compact_Ledge *other_reference_ledge, + IVP_U_BigVector<IVP_Compact_Ledge> *resulting_ledges) +{ + if ( !root_ledge ) + { + IVP_Compact_Ledge *pLedges[2]; + int count = m_pMesh->GetRootLedges( pLedges, ARRAYSIZE(pLedges) ); + if ( count ) + { + for ( int i = 0; i < count; i++ ) + { + resulting_ledges->add( pLedges[i] ); // return the recursive/virtual outer hull + } + return; + } + } + m_pMesh->GetAllLedgesWithinRadius( observer_os, radius, resulting_ledges, root_ledge ); +} + +//----------------------------------------------------------------------------- +// Purpose: Query all of the ledges (triangles) +//----------------------------------------------------------------------------- +void IVP_SurfaceManager_VirtualMesh::get_all_terminal_ledges(IVP_U_BigVector<IVP_Compact_Ledge> *resulting_ledges) +{ + m_pMesh->GetAllLedges( *resulting_ledges ); +} + + + diff --git a/vphysics/physics_virtualmesh.h b/vphysics/physics_virtualmesh.h new file mode 100644 index 0000000..3545ed7 --- /dev/null +++ b/vphysics/physics_virtualmesh.h @@ -0,0 +1,19 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//============================================================================= + +#ifndef PHYSICS_VIRTUALMESH_H +#define PHYSICS_VIRTUALMESH_H +#ifdef _WIN32 +#pragma once +#endif + + +CPhysCollide *CreateVirtualMesh( const virtualmeshparams_t ¶ms ); +void DestroyVirtualMesh( CPhysCollide *pSurf ); +void DumpVirtualMeshStats(); +void VirtualMeshPSI(); + +#endif // PHYSICS_VIRTUALMESH_H diff --git a/vphysics/stdafx.cpp b/vphysics/stdafx.cpp new file mode 100644 index 0000000..a336353 --- /dev/null +++ b/vphysics/stdafx.cpp @@ -0,0 +1,9 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: precompiled header for vphysics +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" + diff --git a/vphysics/trace.cpp b/vphysics/trace.cpp new file mode 100644 index 0000000..c05bc5e --- /dev/null +++ b/vphysics/trace.cpp @@ -0,0 +1,2474 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" +#include "cmodel.h" +#include "physics_trace.h" +#include "ivp_surman_polygon.hxx" +#include "ivp_compact_ledge.hxx" +#include "ivp_compact_ledge_solver.hxx" +#include "ivp_compact_surface.hxx" +#include "tier0/vprof.h" +#include "mathlib/ssemath.h" +#include "tier0/tslist.h" +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +// this skips the sphere tree stuff for tracing +#define DEBUG_TEST_ALL_LEDGES 0 +// this skips the optimization that shrinks the ray as each intersection is encountered +#define DEBUG_KEEP_FULL_RAY 0 +// this skips the optimization that looks up the first vert in a cubemap +#define USE_COLLIDE_MAP 1 + +// objects with small numbers of verts build a cache of pre-transformed verts +#define USE_VERT_CACHE 1 +#define USE_RLE_SPANS 1 + +// UNDONE: This is a boost on PC, but doesn't work yet on x360 - investigate +#define SIMD_MATRIX 0 + +// turn this on to get asserts in the low-level collision solver +#define CHECK_TOI_CALCS 0 + +#define BRUTE_FORCE_VERT_COUNT 128 + +// NOTE: This is in inches (HL units) +#define TEST_EPSILON (g_PhysicsUnits.collisionSweepIncrementalEpsilon) + +struct simplexvert_t +{ + Vector position; + unsigned short testIndex : 15; + unsigned short sweepIndex : 1; + unsigned short obstacleIndex; +}; + +struct simplex_t +{ + simplexvert_t verts[4]; + int vertCount; + + inline bool PointSimplex( const simplexvert_t &newPoint, Vector *pOut ); + inline bool EdgeSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &edge, Vector *pOut ); + inline bool TriangleSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &faceNormal, Vector *pOut ); + + bool SolveGJKSet( const simplexvert_t &newPoint, Vector *pOut ); + bool SolveVoronoiRegion2( const simplexvert_t &newPoint, Vector *pOut ); + bool SolveVoronoiRegion3( const simplexvert_t &newPoint, Vector *pOut ); + bool SolveVoronoiRegion4( const simplexvert_t &newPoint, Vector *pOut ); + + Vector ClipRayToTetrahedronBase( const Vector &dir ); + Vector ClipRayToTetrahedron( const Vector &dir ); + float ClipRayToTriangle( const Vector &dir, float epsilon ); +}; + +class CTraceCone : public ITraceObject +{ +public: + CTraceCone( const truncatedcone_t &cone, const Vector &translation ) + { + m_cone = cone; + m_cone.origin += translation; + float cosTheta; + SinCos( DEG2RAD(m_cone.theta), &m_sinTheta, &cosTheta ); + m_radius = m_cone.h * m_sinTheta / cosTheta; + m_centerBase = m_cone.origin + m_cone.h * m_cone.normal; + } + + virtual int SupportMap( const Vector &dir, Vector *pOut ) const + { + Vector unitDir = dir; + VectorNormalize(unitDir); + + float dot = DotProduct( unitDir, m_cone.normal ); + + // anti-cone is -normal, angle = 90 - theta + // If the normal is in the anti-cone, then return the apex + + // not in anti-cone, support map is on the surface of the disc + if ( dot > -m_sinTheta ) + { + unitDir -= m_cone.normal * dot; + float len = VectorNormalize( unitDir ); + if ( len > 1e-4f ) + { + *pOut = m_centerBase + (unitDir * m_radius); + return 0; + } + *pOut = m_centerBase; + return 0; + + + } + // outside the cone's angle, support map is on the surface of the cone + *pOut = m_cone.origin; + return 0; + } + + // BUGBUG: Doesn't work! + virtual Vector GetVertByIndex( int index ) const { return m_cone.origin; } + virtual float Radius( void ) const { return m_cone.h + m_radius; } + + truncatedcone_t m_cone; + float m_radius; + float m_sinTheta; + Vector m_centerBase; +}; + + +// really this is indexing a vertex, but the iteration code needs a triangle + edge index. +// edge is always 0-2 so return it in the bottom 2 bits +static unsigned short GetPackedIndex( const IVP_Compact_Ledge *pLedge, const IVP_U_Float_Point &dir ) +{ + const IVP_Compact_Poly_Point *RESTRICT pPoints = pLedge->get_point_array(); + const IVP_Compact_Triangle *RESTRICT pTri = pLedge->get_first_triangle(); + const IVP_Compact_Edge *RESTRICT pEdge = pTri->get_edge( 0 ); + int best = pEdge->get_start_point_index(); + float bestDot = pPoints[best].dot_product( &dir ); + int triCount = pLedge->get_n_triangles(); + const IVP_Compact_Triangle *RESTRICT pBestTri = pTri; + // this loop will early out, but keep it from being infinite + int i; + // hillclimbing search to find the best support vert + for ( i = 0; i < triCount; i++ ) + { + // get the index to the end vert of this edge (start vert on next edge) + pEdge = pEdge->get_prev(); + int stopVert = pEdge->get_start_point_index(); + + // loop through the verts that can be reached along edges from this vert + // stop if you get back to the one you're starting on. + int vert = stopVert; + do + { + float dot = pPoints[vert].dot_product( &dir ); + if ( dot > bestDot ) + { + bestDot = dot; + best = vert; + pBestTri = pEdge->get_triangle(); + break; + } + // tri opposite next edge, same starting vert as next edge + pEdge = pEdge->get_opposite()->get_prev(); + vert = pEdge->get_start_point_index(); + } while ( vert != stopVert ); + + // if you exhausted the possibilities for this vert, it must be the best vert + if ( vert != best ) + break; + } + + int triIndex = pBestTri - pLedge->get_first_triangle(); + int edgeIndex = 0; + // just do a search for the edge containing this vert instead of storing it along the way + for ( i = 0; i < 3; i++ ) + { + if ( pBestTri->get_edge(i)->get_start_point_index() == best ) + { + edgeIndex = i; + break; + } + } + + return (unsigned short) ( (triIndex<<2) + edgeIndex ); +} + + +void InitLeafmap( IVP_Compact_Ledge *pLedge, leafmap_t *pLeafmapOut ) +{ + pLeafmapOut->pLeaf = pLedge; + pLeafmapOut->vertCount = 0; + pLeafmapOut->flags = 0; + pLeafmapOut->spanCount = 0; + if ( pLedge && pLedge->is_terminal() ) + { + // for small numbers of verts it's much faster to simply do dot products with all verts + // since the best case for hillclimbing is to touch the start vert plus all neighbors (avg_valence+1 dots) + // in t + int triCount = pLedge->get_n_triangles(); + // this is a guess that anything with more than brute_force * 4 tris will have at least brute_force verts + if ( triCount <= BRUTE_FORCE_VERT_COUNT*4 ) + { + Assert(triCount>0); + int minV = MAX_CONVEX_VERTS; + int maxV = 0; + for ( int i = 0; i < triCount; i++ ) + { + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i; + for ( int j = 0; j < 3; j++ ) + { + const IVP_Compact_Edge *pEdge = pTri->get_edge( j ); + int v = pEdge->get_start_point_index(); + if ( v < minV ) + { + minV = v; + } + if ( v > maxV ) + { + maxV = v; + } + } + } + int vertCount = (maxV-minV) + 1; + // max possible verts is < 48, so this is just here for some real failure + // or vert sharing with a large collection of convexes. In that case the + // number could be high, but this approach to implementing support is invalid + // because the vert range is polluted + if ( vertCount < BRUTE_FORCE_VERT_COUNT ) + { + char hasVert[BRUTE_FORCE_VERT_COUNT]; + memset(hasVert, 0, sizeof(hasVert[0])*vertCount); + for ( int i = 0; i < triCount; i++ ) + { + const IVP_Compact_Triangle *pTri = pLedge->get_first_triangle() + i; + for ( int j = 0; j < 3; j++ ) + { + // mark each vert in the list + const IVP_Compact_Edge *pEdge = pTri->get_edge( j ); + int v = pEdge->get_start_point_index(); + hasVert[v-minV] = true; + } + } + // now find the vertex spans and encode them + byte spans[BRUTE_FORCE_VERT_COUNT]; + int spanIndex = 0; + char has = hasVert[0]; + Assert(has); + byte count = 1; + for ( int i = 1; i < vertCount && spanIndex < BRUTE_FORCE_VERT_COUNT; i++ ) + { + // each change of state is a new span + if ( has != hasVert[i] ) + { + spans[spanIndex] = count; + has = hasVert[i]; + count = 0; + spanIndex++; + } + count++; + Assert(count < 255); + } + + // rle spans only supported with vertex caching +#if USE_VERT_CACHE && USE_RLE_SPANS + if ( spanIndex < BRUTE_FORCE_VERT_COUNT ) +#else + if ( spanIndex < 1 ) +#endif + { + spans[spanIndex] = count; + spanIndex++; + pLeafmapOut->SetRLESpans( minV, spanIndex, spans ); + } + } + } + } + + if ( !pLeafmapOut->HasSpans() ) + { + // otherwise make a 8-way directional map to pick the best start vert for hillclimbing + pLeafmapOut->SetHasCubemap(); + for ( int i = 0; i < 8; i++ ) + { + IVP_U_Float_Point tmp; + tmp.k[0] = ( i & 1 ) ? -1 : 1; + tmp.k[1] = ( i & 2 ) ? -1 : 1; + tmp.k[2] = ( i & 4 ) ? -1 : 1; + pLeafmapOut->startVert[i] = GetPackedIndex( pLedge, tmp ); + } + } +} + + +void GetStartVert( const leafmap_t *pLeafmap, const IVP_U_Float_Point &localDirection, int &triIndex, int &edgeIndex ) +{ + if ( !pLeafmap || !pLeafmap->HasCubemap() ) + return; + + // map dir to index + int cacheIndex = (localDirection.k[0] < 0 ? 1 : 0) + (localDirection.k[1] < 0 ? 2 : 0) + (localDirection.k[2] < 0 ? 4 : 0 ); + triIndex = pLeafmap->startVert[cacheIndex] >> 2; + edgeIndex = pLeafmap->startVert[cacheIndex] & 0x3; +} + +CTSPool<CVisitHash> g_VisitHashPool; + +CVisitHash *AllocVisitHash() +{ + return g_VisitHashPool.GetObject(); +} + +void FreeVisitHash(CVisitHash *pFree) +{ + if ( pFree ) + { + g_VisitHashPool.PutObject(pFree); + } +} + +//----------------------------------------------------------------------------- +// Purpose: Implementation for Trace against an IVP object +//----------------------------------------------------------------------------- +class CTraceIVP : public ITraceObject +{ +public: + CTraceIVP( const CPhysCollide *pCollide, const Vector &origin, const QAngle &angles ); + ~CTraceIVP() + { + if ( m_pVisitHash ) + FreeVisitHash(m_pVisitHash); + } + virtual int SupportMap( const Vector &dir, Vector *pOut ) const; + virtual Vector GetVertByIndex( int index ) const; + + // UNDONE: Do general ITraceObject center/offset computation and move the ray to account + // for this delta like we do in TraceSweepIVP() + // Then we can shrink the radius of objects with mass centers NOT at the origin + virtual float Radius( void ) const + { + return m_radius; + } + + inline float TransformLengthToLocal( float length ) + { + return ConvertDistanceToIVP( length ); + } + // UNDONE: Optimize this by storing 3 matrices? (one for each transform that includes rot/scale for HL/IVP)? + // UNDONE: Not necessary if we remove the coordinate conversion + inline void TransformDirectionToLocal( const Vector &dir, IVP_U_Float_Point &local ) const + { + IVP_U_Float_Point tmp; + ConvertDirectionToIVP( dir, tmp ); + m_matrix.vimult3( &tmp, &local ); + } + + inline void RotateRelativePositionToLocal( const Vector &delta, IVP_U_Float_Point &local ) const + { + IVP_U_Float_Point tmp; + ConvertPositionToIVP( delta, tmp ); + m_matrix.vimult3( &tmp, &local ); + } + + inline void TransformPositionToLocal( const Vector &pos, IVP_U_Float_Point &local ) const + { + IVP_U_Float_Point tmp; + ConvertPositionToIVP( pos, tmp ); + m_matrix.vimult4( &tmp, &local ); + } + + inline void TransformPositionFromLocal( const IVP_U_Float_Point &local, Vector &out ) const + { + VectorTransform( *(Vector *)&local, *((const matrix3x4_t *)&m_ivpLocalToHLWorld), out ); + } + +#if USE_VERT_CACHE + inline Vector CachedVertByIndex(int index) const + { + int subIndex = index & 3; + return m_vertCache[index>>2].Vec(subIndex); + } +#endif + + bool IsValid( void ) { return m_pLedge != NULL; } + + void AllocateVisitHash() + { + if ( !m_pVisitHash ) + m_pVisitHash = AllocVisitHash(); + } + + void SetLedge( const IVP_Compact_Ledge *pLedge ) + { + m_pLedge = pLedge; + m_pLeafmap = NULL; + if ( !pLedge ) + return; + +#if USE_VERT_CACHE + m_cacheCount = 0; +#endif + if ( m_pCollideMap ) + { + for ( int i = 0; i < m_pCollideMap->leafCount; i++ ) + { + if ( m_pCollideMap->leafmap[i].pLeaf == pLedge ) + { + m_pLeafmap = &m_pCollideMap->leafmap[i]; + if ( !BuildLeafmapCache( &m_pCollideMap->leafmap[i] ) ) + { + AllocateVisitHash(); + } + return; + } + } + } + AllocateVisitHash(); + } + + bool SetSingleConvex( void ) + { + const IVP_Compact_Ledgetree_Node *node = m_pSurface->get_compact_ledge_tree_root(); + if ( node->is_terminal() == IVP_TRUE ) + { + SetLedge( node->get_compact_ledge() ); + return true; + } + SetLedge( NULL ); + return false; + } + bool BuildLeafmapCache(const leafmap_t * RESTRICT pLeafmap); + bool BuildLeafmapCacheRLE( const leafmap_t * RESTRICT pLeafmap ); + inline int SupportMapCached( const Vector &dir, Vector *pOut ) const; + const collidemap_t *m_pCollideMap; + const IVP_Compact_Surface *m_pSurface; + +private: + const leafmap_t *m_pLeafmap; + const IVP_Compact_Ledge *m_pLedge; + CVisitHash *m_pVisitHash; +#if SIMD_MATRIX + FourVectors m_ivpLocalToHLWorld; +#else + matrix3x4_t m_ivpLocalToHLWorld; +#endif + IVP_U_Matrix m_matrix; + // transform that includes scale from IVP to HL coords, do not VectorITransform or VectorRotate with this + float m_radius; + int m_nPointTest; + int m_nStartPoint; + bool m_bHasTranslation; +#if USE_VERT_CACHE + int m_cacheCount; // number of FourVectors used + FourVectors m_vertCache[BRUTE_FORCE_VERT_COUNT/4]; +#endif +}; + +// GCC 4.2.1 can't handle loading a static const into a m128 register :( +#ifdef WIN32 +static const +#endif +fltx4 g_IVPToHLDir = { 1.0f, -1.0f, 1.0f, 1.0f }; + +//static const fltx4 g_IVPToHLPosition = { IVP2HL(1.0f), -IVP2HL(1.0f), IVP2HL(1.0f), IVP2HL(1.0f) }; + +#if defined(_X360) + +FORCEINLINE fltx4 ConvertDirectionToIVP( const fltx4 & a ) +{ + fltx4 t = __vpermwi( a, VPERMWI_CONST( 0, 2, 1, 3 ) ); + // negate Y + return MulSIMD( t, g_IVPToHLDir ); +} +#else +FORCEINLINE fltx4 ConvertDirectionToIVP( const fltx4 & a ) +{ + // swap Z & Y + fltx4 t = _mm_shuffle_ps( a, a, MM_SHUFFLE_REV( 0, 2, 1, 3 ) ); + // negate Y + return MulSIMD( t, g_IVPToHLDir ); +} +#endif + +CTraceIVP::CTraceIVP( const CPhysCollide *pCollide, const Vector &origin, const QAngle &angles ) +{ +#if USE_COLLIDE_MAP + m_pCollideMap = pCollide->GetCollideMap(); +#else + m_pCollideMap = NULL; +#endif + m_pSurface = pCollide->GetCompactSurface(); + m_pLedge = NULL; + m_pVisitHash = NULL; + + m_bHasTranslation = (origin==vec3_origin) ? false : true; + // UNDONE: Move this offset calculation into the tracing routines + // I didn't do this now because it seems to require changes to most of the + // transform routines - and this would cause bugs. + float centerOffset = VectorLength( m_pSurface->mass_center.k ); +#if SIMD_MATRIX + VectorAligned forward, right, up; + IVP_U_Float_Point ivpForward, ivpLeft, ivpUp; + + AngleVectors( angles, &forward, &right, &up ); + + Vector left = -right; + Vector down = -up; + + ConvertDirectionToIVP( forward, ivpForward ); + ConvertDirectionToIVP( left, ivpLeft ); + ConvertDirectionToIVP( down, ivpUp ); + + m_matrix.set_col( IVP_INDEX_X, &ivpForward ); + m_matrix.set_col( IVP_INDEX_Z, &ivpLeft ); + m_matrix.set_col( IVP_INDEX_Y, &ivpUp ); + ConvertPositionToIVP( origin, m_matrix.vv ); + + forward.w = HL2IVP(origin.x); + // This vector is supposed to be left, so we'll negate it later, but we don't want to + // negate the position, so add another minus to cancel out + right.w = -HL2IVP(origin.y); + up.w = HL2IVP(origin.z); + fltx4 rx = ConvertDirectionToIVP(LoadAlignedSIMD(forward.Base())); + fltx4 ry = ConvertDirectionToIVP(SubSIMD( Four_Zeros, LoadAlignedSIMD(right.Base())) ); + fltx4 rz = ConvertDirectionToIVP(LoadAlignedSIMD(up.Base()) ); + + fltx4 scaleHL = ReplicateX4(IVP2HL(1.0f)); + m_ivpLocalToHLWorld.x = MulSIMD( scaleHL, rx ); + m_ivpLocalToHLWorld.y = MulSIMD( scaleHL, ry ); + m_ivpLocalToHLWorld.z = MulSIMD( scaleHL, rz ); +#else + ConvertRotationToIVP( angles, m_matrix ); + ConvertPositionToIVP( origin, m_matrix.vv ); + float scale = IVP2HL(1.0f); + float negScale = IVP2HL(-1.0f); + // copy the existing IVP local->world matrix (swap Y & Z) + m_ivpLocalToHLWorld.m_flMatVal[0][0] = m_matrix.get_elem(IVP_INDEX_X,0) * scale; + m_ivpLocalToHLWorld.m_flMatVal[0][1] = m_matrix.get_elem(IVP_INDEX_X,1) * scale; + m_ivpLocalToHLWorld.m_flMatVal[0][2] = m_matrix.get_elem(IVP_INDEX_X,2) * scale; + + m_ivpLocalToHLWorld.m_flMatVal[1][0] = m_matrix.get_elem(IVP_INDEX_Z,0) * scale; + m_ivpLocalToHLWorld.m_flMatVal[1][1] = m_matrix.get_elem(IVP_INDEX_Z,1) * scale; + m_ivpLocalToHLWorld.m_flMatVal[1][2] = m_matrix.get_elem(IVP_INDEX_Z,2) * scale; + + m_ivpLocalToHLWorld.m_flMatVal[2][0] = m_matrix.get_elem(IVP_INDEX_Y,0) * negScale; + m_ivpLocalToHLWorld.m_flMatVal[2][1] = m_matrix.get_elem(IVP_INDEX_Y,1) * negScale; + m_ivpLocalToHLWorld.m_flMatVal[2][2] = m_matrix.get_elem(IVP_INDEX_Y,2) * negScale; + + m_ivpLocalToHLWorld.m_flMatVal[0][3] = m_matrix.vv.k[0] * scale; + m_ivpLocalToHLWorld.m_flMatVal[1][3] = m_matrix.vv.k[2] * scale; + m_ivpLocalToHLWorld.m_flMatVal[2][3] = m_matrix.vv.k[1] * negScale; + +#endif + + m_radius = ConvertDistanceToHL( m_pSurface->upper_limit_radius + centerOffset ); +} + +bool CTraceIVP::BuildLeafmapCacheRLE( const leafmap_t * RESTRICT pLeafmap ) +{ + // iterate the rle spans of verts and output them to a buffer in post-transform space + int startPoint = pLeafmap->startVert[0]; + int pointCount = pLeafmap->vertCount; + m_cacheCount = (pointCount + 3)>>2; + const byte *RESTRICT pSpans = pLeafmap->GetSpans(); + int countThisSpan = pSpans[0]; + int spanIndex = 1; + int baseVert = 0; + const VectorAligned * RESTRICT pVerts = (const VectorAligned *)&m_pLedge->get_point_array()[startPoint]; + for ( int i = 0; i < m_cacheCount-1; i++ ) + { + if ( countThisSpan < 4 ) + { + // unrolled for perf + // we need a batch of four verts, but they aren't in a single span + int v0, v1, v2, v3; + if ( !countThisSpan ) + { + baseVert += pSpans[spanIndex]; + countThisSpan = pSpans[spanIndex+1]; + spanIndex += 2; + } + v0 = baseVert++; + countThisSpan--; + if ( !countThisSpan ) + { + baseVert += pSpans[spanIndex]; + countThisSpan = pSpans[spanIndex+1]; + spanIndex += 2; + } + v1 = baseVert++; + countThisSpan--; + if ( !countThisSpan ) + { + baseVert += pSpans[spanIndex]; + countThisSpan = pSpans[spanIndex+1]; + spanIndex += 2; + } + v2 = baseVert++; + countThisSpan--; + if ( !countThisSpan ) + { + baseVert += pSpans[spanIndex]; + countThisSpan = pSpans[spanIndex+1]; + spanIndex += 2; + } + v3 = baseVert++; + countThisSpan--; + m_vertCache[i].LoadAndSwizzleAligned( pVerts[v0].Base(), pVerts[v1].Base(), pVerts[v2].Base(), pVerts[v3].Base() ); + } + else + { + // we have four verts in this span, just grab the next four + m_vertCache[i].LoadAndSwizzleAligned( pVerts[baseVert+0].Base(), pVerts[baseVert+1].Base(), pVerts[baseVert+2].Base(), pVerts[baseVert+3].Base() ); + baseVert += 4; + countThisSpan -= 4; + } + } + // the last iteration needs multiple spans and clamping to the last vert + int v[4]; + for ( int i = 0; i < 4; i++ ) + { + if ( spanIndex < pLeafmap->spanCount && !countThisSpan ) + { + baseVert += pSpans[spanIndex]; + countThisSpan = pSpans[spanIndex+1]; + spanIndex += 2; + } + if ( spanIndex < pLeafmap->spanCount ) + { + v[i] = baseVert; + baseVert++; + countThisSpan--; + } + else + { + v[i] = baseVert; + if ( countThisSpan > 1 ) + { + countThisSpan--; + baseVert++; + } + } + } + m_vertCache[m_cacheCount-1].LoadAndSwizzleAligned( pVerts[v[0]].Base(), pVerts[v[1]].Base(), pVerts[v[2]].Base(), pVerts[v[3]].Base() ); + FourVectors::RotateManyBy( &m_vertCache[0], m_cacheCount, *((const matrix3x4_t *)&m_ivpLocalToHLWorld) ); + + return true; +} + +bool CTraceIVP::BuildLeafmapCache( const leafmap_t * RESTRICT pLeafmap ) +{ +#if !USE_VERT_CACHE + return false; +#else + if ( !pLeafmap || !pLeafmap->HasSpans() || m_bHasTranslation ) + return false; + if ( pLeafmap->HasRLESpans() ) + { + return BuildLeafmapCacheRLE(pLeafmap); + } + + // single vertex span, just xform + copy + + // iterate the span of verts and output them to a buffer in post-transform space + // just iterate the range if one is specified + int startPoint = pLeafmap->startVert[0]; + int pointCount = pLeafmap->vertCount; + m_cacheCount = (pointCount + 3)>>2; + Assert(m_cacheCount>=0 && m_cacheCount<= (BRUTE_FORCE_VERT_COUNT/4)); + + const VectorAligned * RESTRICT pVerts = (const VectorAligned *)&m_pLedge->get_point_array()[startPoint]; + for ( int i = 0; i < m_cacheCount-1; i++ ) + { + m_vertCache[i].LoadAndSwizzleAligned( pVerts[0].Base(), pVerts[1].Base(), pVerts[2].Base(), pVerts[3].Base() ); + pVerts += 4; + } + + int remIndex = (pointCount-1) & 3; + int x0 = 0; + int x1 = min(1,remIndex); + int x2 = min(2,remIndex); + int x3 = min(3,remIndex); + m_vertCache[m_cacheCount-1].LoadAndSwizzleAligned( pVerts[x0].Base(), pVerts[x1].Base(), pVerts[x2].Base(), pVerts[x3].Base() ); + FourVectors::RotateManyBy( &m_vertCache[0], m_cacheCount, *((const matrix3x4_t *)&m_ivpLocalToHLWorld) ); + return true; +#endif +} + +static const fltx4 g_IndexBase = {0,1,2,3}; +int CTraceIVP::SupportMapCached( const Vector &dir, Vector *pOut ) const +{ + VPROF("SupportMapCached"); +#if USE_VERT_CACHE + FourVectors fourDir; +#if defined(_X360) + fltx4 vec = LoadUnaligned3SIMD( dir.Base() ); + fourDir.x = SplatXSIMD(vec); + fourDir.y = SplatYSIMD(vec); + fourDir.z = SplatZSIMD(vec); +#else + fourDir.DuplicateVector(dir); +#endif + + fltx4 index = g_IndexBase; + fltx4 maxIndex = g_IndexBase; + fltx4 maxDot = fourDir * m_vertCache[0]; + for ( int i = 1; i < m_cacheCount; i++ ) + { + index = AddSIMD(index, Four_Fours); + fltx4 dot = fourDir * m_vertCache[i]; + fltx4 cmpMask = CmpGtSIMD(dot,maxDot); + maxIndex = MaskedAssign( cmpMask, index, maxIndex ); + maxDot = MaxSIMD(dot, maxDot); + } + + // find highest of 4 + fltx4 rot = RotateLeft2(maxDot); + fltx4 rotIndex = RotateLeft2(maxIndex); + fltx4 cmpMask = CmpGtSIMD(rot,maxDot); + maxIndex = MaskedAssign(cmpMask, rotIndex, maxIndex); + maxDot = MaxSIMD(rot,maxDot); + rotIndex = RotateLeft(maxIndex); + rot = RotateLeft(maxDot); + cmpMask = CmpGtSIMD(rot,maxDot); + maxIndex = MaskedAssign(cmpMask, rotIndex, maxIndex); + // not needed unless we need the actual max dot at the end + // maxDot = MaxSIMD(rot,maxDot); + + int bestIndex = SubFloatConvertToInt(maxIndex,0); + *pOut = CachedVertByIndex(bestIndex); + + return bestIndex; +#else + Assert(0); +#endif +} + +int CTraceIVP::SupportMap( const Vector &dir, Vector *pOut ) const +{ +#if USE_VERT_CACHE + if ( m_cacheCount ) + return SupportMapCached( dir, pOut ); +#endif + + if ( m_pLeafmap && m_pLeafmap->HasSingleVertexSpan() ) + { + VPROF("SupportMap_Leaf"); + const IVP_U_Float_Point *pPoints = m_pLedge->get_point_array(); + IVP_U_Float_Point mapdir; + TransformDirectionToLocal( dir, mapdir ); + // just iterate the range if one is specified + int startPoint = m_pLeafmap->startVert[0]; + int pointCount = m_pLeafmap->vertCount; + float bestDot = pPoints[startPoint].dot_product(&mapdir); + int best = startPoint; + for ( int i = 1; i < pointCount; i++ ) + { + float dot = pPoints[startPoint+i].dot_product(&mapdir); + if ( dot > bestDot ) + { + bestDot = dot; + best = startPoint+i; + } + } + + TransformPositionFromLocal( pPoints[best], *pOut ); // transform point position to world space + return best; + } + else + { + VPROF("SupportMap_Walk"); + const IVP_U_Float_Point *pPoints = m_pLedge->get_point_array(); + IVP_U_Float_Point mapdir; + TransformDirectionToLocal( dir, mapdir ); + int triCount = m_pLedge->get_n_triangles(); + Assert( m_pVisitHash ); + m_pVisitHash->NewVisit(); + + float dot; + int triIndex = 0, edgeIndex = 0; + GetStartVert( m_pLeafmap, mapdir, triIndex, edgeIndex ); + const IVP_Compact_Triangle *RESTRICT pTri = m_pLedge->get_first_triangle() + triIndex; + const IVP_Compact_Edge *RESTRICT pEdge = pTri->get_edge( edgeIndex ); + int best = pEdge->get_start_point_index(); + float bestDot = pPoints[best].dot_product( &mapdir ); + m_pVisitHash->VisitVert(best); + + // This should never happen. MAX_CONVEX_VERTS is very large (millions), none of our + // models have anywhere near this many verts in a convex piece + Assert(triCount*3<MAX_CONVEX_VERTS); + // this loop will early out, but keep it from being infinite + for ( int i = 0; i < triCount; i++ ) + { + // get the index to the end vert of this edge (start vert on next edge) + pEdge = pEdge->get_prev(); + int stopVert = pEdge->get_start_point_index(); + + // loop through the verts that can be reached along edges from this vert + // stop if you get back to the one you're starting on. + int vert = stopVert; + do + { + if ( !m_pVisitHash->WasVisited(vert) ) + { + // this lets us skip doing dot products on this vert + m_pVisitHash->VisitVert(vert); + dot = pPoints[vert].dot_product( &mapdir ); + if ( dot > bestDot ) + { + bestDot = dot; + best = vert; + break; + } + } + // tri opposite next edge, same starting vert as next edge + pEdge = pEdge->get_opposite()->get_prev(); + vert = pEdge->get_start_point_index(); + } while ( vert != stopVert ); + + // if you exhausted the possibilities for this vert, it must be the best vert + if ( vert != best ) + break; + } + + // code to do the brute force method with no hill-climbing +#if 0 + for ( i = 0; i < triCount; i++ ) + { + pTri = m_pLedge->get_first_triangle() + i; + for ( int j = 0; j < 3; j++ ) + { + pEdge = pTri->get_edge( j ); + int test = pEdge->get_start_point_index(); + dot = pPoints[test].dot_product( &mapdir ); + if ( dot > bestDot ) + { + Assert(0); // shouldn't hit this unless the hill-climb is broken + bestDot = dot; + best = test; + } + } + } +#endif + TransformPositionFromLocal( pPoints[best], *pOut ); // transform point position to world space + + return best; + } +} + +Vector CTraceIVP::GetVertByIndex( int index ) const +{ +#if USE_VERT_CACHE + if ( m_cacheCount ) + { + return CachedVertByIndex(index); + } +#endif + const IVP_Compact_Poly_Point *pPoints = m_pLedge->get_point_array(); + + Vector out; + TransformPositionFromLocal( pPoints[index], out ); + return out; +} + +//----------------------------------------------------------------------------- +// Purpose: Implementation for Trace against an AABB +//----------------------------------------------------------------------------- +class CTraceAABB : public ITraceObject +{ +public: + CTraceAABB( const Vector &hlmins, const Vector &hlmaxs, bool isPoint ); + virtual int SupportMap( const Vector &dir, Vector *pOut ) const; + virtual Vector GetVertByIndex( int index ) const; + virtual float Radius( void ) const { return m_radius; } + +private: + float m_x[2]; + float m_y[2]; + float m_z[2]; + float m_radius; + bool m_empty; +}; + + +CTraceAABB::CTraceAABB( const Vector &hlmins, const Vector &hlmaxs, bool isPoint ) +{ + if ( isPoint ) + { + m_x[0] = m_x[1] = 0; + m_y[0] = m_y[1] = 0; + m_z[0] = m_z[1] = 0; + m_radius = 0; + m_empty = true; + } + else + { + m_x[0] = hlmaxs[0]; + m_x[1] = hlmins[0]; + m_y[0] = hlmaxs[1]; + m_y[1] = hlmins[1]; + m_z[0] = hlmaxs[2]; + m_z[1] = hlmins[2]; + m_radius = hlmaxs.Length(); + m_empty = false; + } +} + + +int CTraceAABB::SupportMap( const Vector &dir, Vector *pOut ) const +{ + Vector out; + + if ( m_empty ) + { + pOut->Init(); + return 0; + } + // index is formed by the 3-bit bitfield SzSySx (negative is 1, positive is 0) + int x = ((*((unsigned int *)&dir.x)) & 0x80000000UL) >> 31; + int y = ((*((unsigned int *)&dir.y)) & 0x80000000UL) >> 31; + int z = ((*((unsigned int *)&dir.z)) & 0x80000000UL) >> 31; + pOut->x = m_x[x]; + pOut->y = m_y[y]; + pOut->z = m_z[z]; + return (z<<2) | (y<<1) | x; +} + +Vector CTraceAABB::GetVertByIndex( int index ) const +{ + Vector out; + out.x = m_x[(index&1)]; + out.y = m_y[(index&2)>>1]; + out.z = m_z[(index&4)>>2]; + + return out; +} + + +//----------------------------------------------------------------------------- +// Purpose: Implementation for Trace against an IVP object +//----------------------------------------------------------------------------- +class CTraceRay +{ +public: + CTraceRay( const Vector &hlstart, const Vector &hlend ); + CTraceRay( const Ray_t &ray ); + CTraceRay( const Ray_t &ray, const Vector &offset ); + void Init( const Vector &hlstart, const Vector &delta ); + int SupportMap( const Vector &dir, Vector *pOut ) const; + Vector GetVertByIndex( int index ) const { return ( index ) ? m_end : m_start; } + float Radius( void ) const { return m_length * 0.5f; } + + void Reset( float fraction ); + + Vector m_start; + Vector m_end; + Vector m_delta; + Vector m_dir; + + float m_length; + float m_baseLength; + float m_ooBaseLength; + float m_bestDist; +}; +CTraceRay::CTraceRay( const Vector &hlstart, const Vector &hlend ) +{ + Init(hlstart, hlend-hlstart); +} + +void CTraceRay::Init( const Vector &hlstart, const Vector &delta ) +{ + m_start = hlstart; + m_end = hlstart + delta; + m_delta = delta; + m_dir = delta; + float len = DotProduct(delta, delta); + // don't use fast/sse sqrt here we need the precision + m_length = sqrt(len); + m_ooBaseLength = 0.0f; + if ( m_length > 0 ) + { + m_ooBaseLength = 1.0f / m_length; + m_dir *= m_ooBaseLength; + } + m_baseLength = m_length; + m_bestDist = 0.f; +} + +CTraceRay::CTraceRay( const Ray_t &ray ) +{ + Init( ray.m_Start, ray.m_Delta ); +} + +CTraceRay::CTraceRay( const Ray_t &ray, const Vector &offset ) +{ + Vector start; + VectorAdd( ray.m_Start, offset, start ); + Init( start, ray.m_Delta ); +} + +void CTraceRay::Reset( float fraction ) +{ + // recompute from base values for max precision + m_length = m_baseLength * fraction; + m_end = m_start + fraction * m_delta; + m_bestDist = 0.f; +} + +int CTraceRay::SupportMap( const Vector &dir, Vector *pOut ) const +{ + if ( DotProduct( dir, m_delta ) > 0 ) + { + *pOut = m_end; + return 1; + } + *pOut = m_start; + return 0; +} + +static char *map_nullname = "**empty**"; +static csurface_t nullsurface = { map_nullname, 0 }; + +static void CM_ClearTrace( trace_t *trace ) +{ + memset( trace, 0, sizeof(*trace)); + trace->fraction = 1.f; + trace->fractionleftsolid = 0; + trace->surface = nullsurface; +} + +class CDefConvexInfo : public IConvexInfo +{ +public: + IConvexInfo *GetPtr() { return this; } + + virtual unsigned int GetContents( int convexGameData ) { return CONTENTS_SOLID; } +}; + +class CTraceSolver +{ +public: + CTraceSolver( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, ITraceObject *obstacle, const Vector &axis ) + { + m_pTotalTrace = ptr; + m_sweepObject = sweepobject; + m_sweepObjectRadius = m_sweepObject->Radius(); + m_obstacle = obstacle; + m_ray = ray; + m_traceLength = 0; + m_totalTraceLength = max( ray->m_baseLength, 1e-8f ); + m_pointClosestToIntersection = axis; + m_epsilon = g_PhysicsUnits.collisionSweepEpsilon; + } + + bool SweepSingleConvex( void ); + float SolveMeshIntersection( simplex_t &simplex ); + float SolveMeshIntersection2D( simplex_t &simplex ); + virtual void DoSweep( void ) + { + SweepSingleConvex(); + *m_pTotalTrace = m_trace; + } + + + void SetEpsilon( float epsilon ) + { + m_epsilon = epsilon; + } + +protected: + trace_t m_trace; + + Vector m_pointClosestToIntersection; + ITraceObject *m_sweepObject; + ITraceObject *m_obstacle; + CTraceRay *m_ray; + trace_t *m_pTotalTrace; + float m_traceLength; + float m_totalTraceLength; + float m_sweepObjectRadius; + float m_epsilon; +private: + CTraceSolver( const CTraceSolver & ); +}; + +class CTraceSolverSweptObject : public CTraceSolver +{ +public: + CTraceSolverSweptObject( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, CTraceIVP *obstacle, const Vector &axis, unsigned int contentsMask, IConvexInfo *pConvexInfo ); + + void InitOSRay( void ); + void SweepLedgeTree_r( const IVP_Compact_Ledgetree_Node *node ); + inline bool SweepHitsSphereOS( const IVP_U_Float_Point *sphereCenter, float radius ); + virtual void DoSweep( void ); + inline void SweepAgainstNode( const IVP_Compact_Ledgetree_Node *node ); + + CTraceIVP *m_obstacleIVP; + IConvexInfo *m_pConvexInfo; + unsigned int m_contentsMask; + CDefConvexInfo m_fakeConvexInfo; + + + IVP_U_Float_Point m_rayCenterOS; + IVP_U_Float_Point m_rayStartOS; + IVP_U_Float_Point m_rayDirOS; + IVP_U_Float_Point m_rayDeltaOS; + float m_rayLengthOS; + +private: + CTraceSolverSweptObject( const CTraceSolverSweptObject & ); // no implementation, quells compiler warning +}; + +CTraceSolverSweptObject::CTraceSolverSweptObject( trace_t *ptr, ITraceObject *sweepobject, CTraceRay *ray, CTraceIVP *obstacle, const Vector &axis, unsigned int contentsMask, IConvexInfo *pConvexInfo ) +: CTraceSolver( ptr, sweepobject, ray, obstacle, axis ) +{ + m_obstacleIVP = obstacle; + m_contentsMask = contentsMask; + m_pConvexInfo = (pConvexInfo != NULL) ? pConvexInfo : m_fakeConvexInfo.GetPtr(); +} + + +bool CTraceSolverSweptObject::SweepHitsSphereOS( const IVP_U_Float_Point *sphereCenter, float radius ) +{ + // disable this to help find bugs +#if DEBUG_TEST_ALL_LEDGES + return true; +#endif + // the ray is actually a line-swept-sphere with sweep object's radius + IVP_U_Float_Point delta_vec; // quick check for ends of ray + delta_vec.subtract( sphereCenter, &m_rayCenterOS ); + radius += m_sweepObjectRadius; + // Is the sphere close enough to the ray at the center? + float qsphere_rad = radius * radius; + + // If this is a 0 length ray, then the conservative test is 100% accurate + if ( m_rayLengthOS > 0 ) + { + // Calculate the perpendicular distance to the sphere + // The perpendicular forms a right triangle with the vector between the ray/sphere centers + // and the ray direction vector. Calculate the projection of the hypoteneuse along the perpendicular + IVP_U_Float_Point h; + h.inline_calc_cross_product(&m_rayDirOS, &delta_vec); + + if( h.quad_length() < qsphere_rad ) + return true; + } + else + { + float quad_center_dist = delta_vec.quad_length(); + + if ( quad_center_dist < qsphere_rad ) + { + return true; + } + + // Could a ray in any direction away from the ray center intersect this sphere? + float qrad_sum = m_rayLengthOS * 0.5f + radius; + qrad_sum *= qrad_sum; + if ( quad_center_dist >= qrad_sum ) + { + return false; + } + } + + return false; +} + +inline void CTraceSolverSweptObject::SweepAgainstNode(const IVP_Compact_Ledgetree_Node *node) +{ + const IVP_Compact_Ledge *ledge = node->get_compact_ledge(); + unsigned int ledgeContents = m_pConvexInfo->GetContents( ledge->get_client_data() ); + if (m_contentsMask & ledgeContents) + { + m_obstacleIVP->SetLedge( ledge ); + if ( SweepSingleConvex() ) + { + if ( m_traceLength < m_totalTraceLength ) + { + m_pTotalTrace->plane.normal = m_trace.plane.normal; + m_pTotalTrace->startsolid = m_trace.startsolid; + m_pTotalTrace->allsolid = m_trace.allsolid; + m_totalTraceLength = m_traceLength; + m_pTotalTrace->fraction = m_traceLength * m_ray->m_ooBaseLength; + Assert(m_pTotalTrace->fraction >= 0 && m_pTotalTrace->fraction <= 1.0f); +#if !DEBUG_KEEP_FULL_RAY + // shrink the ray to the shortened length, but leave a buffer of collisionSweepEpsilon units + // at the end to make sure that precision doesn't make you miss something slightly closer + float testFraction = (m_traceLength + m_epsilon*2) * m_ray->m_ooBaseLength; + if ( testFraction < 1.0f ) + { + m_ray->Reset( testFraction ); + // Update OS ray to limit tests + m_rayLengthOS = m_obstacleIVP->TransformLengthToLocal( m_ray->m_length ); + m_rayCenterOS.add_multiple( &m_rayStartOS, &m_rayDeltaOS, 0.5f * testFraction ); + } +#endif + m_pTotalTrace->contents = ledgeContents; + } + } + } +} + + +void CTraceSolverSweptObject::SweepLedgeTree_r( const IVP_Compact_Ledgetree_Node *node ) +{ + IVP_U_Float_Point center; + center.set(node->center.k); + if ( !SweepHitsSphereOS( ¢er, node->radius ) ) + return; + + // fast path for single leaf collision models + if ( node->is_terminal() == IVP_TRUE ) + { + SweepAgainstNode(node); + return; + } + + // use an array to implement a simple stack + CUtlVectorFixedGrowable<const IVP_Compact_Ledgetree_Node *, 64> list; + + // pull the last item in the array (top of stack) + // this is nearly a priority queue, but not actually, but it's cheaper (and faster in the benchmarks) + // this code is trying to visit the nodes closest to the ray start first - which helps performance + // since we're only interested in the first intersection of the swept object with the physcollide. + while ( 1 ) + { + // don't use the temp storage unless you have to. +loop_without_store: + if ( node->is_terminal() == IVP_TRUE ) + { + // leaf, do the test + SweepAgainstNode(node); + } + else + { + // check node's children + const IVP_Compact_Ledgetree_Node *node0 = node->left_son(); + center.set(node0->center.k); + // if we don't insert, this is larger than any quad distance + float lastDist = 1e24f; + if ( SweepHitsSphereOS( ¢er, node0->radius ) ) + { + lastDist = m_rayStartOS.quad_distance_to(¢er); + } + else + { + node0 = NULL; + } + const IVP_Compact_Ledgetree_Node *node1 = node->right_son(); + center.set(node1->center.k); + if ( SweepHitsSphereOS( ¢er, node1->radius ) ) + { + if ( node0 ) + { + // can hit, push on stack + int index = list.AddToTail(); + float dist1 = m_rayStartOS.quad_distance_to(¢er); + if ( lastDist < dist1 ) + { + node = node0; + list[index] = node1; + } + else + { + node = node1; + list[index] = node0; + } + } + else + { + node = node1; + } + goto loop_without_store; + } + if ( node0 ) + { + node = node0; + goto loop_without_store; + } + } + int last = list.Count()-1; + if ( last < 0 ) + break; + node = list[last]; + list.FastRemove(last); + } +} + + +void CTraceSolverSweptObject::InitOSRay( void ) +{ + // transform ray into object space + m_rayLengthOS = m_obstacleIVP->TransformLengthToLocal( m_ray->m_length ); + m_obstacleIVP->TransformPositionToLocal( m_ray->m_start, m_rayStartOS ); + + // no translation on matrix mult because this is a vector + m_obstacleIVP->RotateRelativePositionToLocal( m_ray->m_delta, m_rayDeltaOS ); + m_rayDirOS.set(&m_rayDeltaOS); + m_rayDirOS.normize(); + + // add_multiple with 3 params assumes no initial value (should be set_add_multiple) + m_rayCenterOS.add_multiple( &m_rayStartOS, &m_rayDeltaOS, 0.5f ); +} + + +void CTraceSolverSweptObject::DoSweep( void ) +{ + VPROF("TraceSolver::DoSweep"); + InitOSRay(); + + // iterate ledge tree of obstacle + const IVP_Compact_Surface *pSurface = m_obstacleIVP->m_pSurface; + + const IVP_Compact_Ledgetree_Node *lt_node_root; + lt_node_root = pSurface->get_compact_ledge_tree_root(); + SweepLedgeTree_r( lt_node_root ); +} + +void CPhysicsTrace::SweepBoxIVP( const Vector &start, const Vector &end, const Vector &mins, const Vector &maxs, const CPhysCollide *pCollide, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr ) +{ + Ray_t ray; + ray.Init( start, end, mins, maxs ); + SweepBoxIVP( ray, MASK_ALL, NULL, pCollide, surfaceOrigin, surfaceAngles, ptr ); +} + +void CPhysicsTrace::SweepBoxIVP( const Ray_t &raySrc, unsigned int contentsMask, IConvexInfo *pConvexInfo, const CPhysCollide *pCollide, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr ) +{ + CM_ClearTrace( ptr ); + + CTraceAABB box( -raySrc.m_Extents, raySrc.m_Extents, raySrc.m_IsRay ); + CTraceIVP ivp( pCollide, vec3_origin, surfaceAngles ); + + // offset the space of this sweep so that the surface is at the origin of the solution space + CTraceRay ray( raySrc, -surfaceOrigin ); + + CTraceSolverSweptObject solver( ptr, &box, &ray, &ivp, ray.m_start, contentsMask, pConvexInfo ); + solver.DoSweep(); + + VectorAdd( raySrc.m_Start, raySrc.m_StartOffset, ptr->startpos ); + VectorMA( ptr->startpos, ptr->fraction, raySrc.m_Delta, ptr->endpos ); + // The plane was shifted because we shifted everything over by surfaceOrigin, shift it back + if ( ptr->DidHit() ) + { + ptr->plane.dist = DotProduct( ptr->endpos, ptr->plane.normal ); + } +} + +void CPhysicsTrace::SweepIVP( const Vector &start, const Vector &end, const CPhysCollide *pSweptSurface, const QAngle &sweptAngles, const CPhysCollide *pSurface, const Vector &surfaceOrigin, const QAngle &surfaceAngles, trace_t *ptr ) +{ + CM_ClearTrace( ptr ); + + CTraceIVP sweptObject( pSweptSurface, vec3_origin, sweptAngles ); + + // offset the space of this sweep so that the surface is at the origin of the solution space + CTraceIVP ivp( pSurface, vec3_origin, surfaceAngles ); + CTraceRay ray( start - surfaceOrigin, end - surfaceOrigin ); + + IVP_U_BigVector<IVP_Compact_Ledge> objectLedges(32); + IVP_Compact_Ledge_Solver::get_all_ledges( pSweptSurface->GetCompactSurface(), &objectLedges ); + for ( int i = objectLedges.len() - 1; i >= 0; --i ) + { + trace_t tr; + CM_ClearTrace( &tr ); + sweptObject.SetLedge( objectLedges.element_at(i) ); + CTraceSolverSweptObject solver( &tr, &sweptObject, &ray, &ivp, start - surfaceOrigin, MASK_ALL, NULL ); + // UNDONE: Need just more than 0.25" tolerance here because the output position will be used by vphysics + // UNDONE: Really this should be the collision radius from the environment. + solver.SetEpsilon( g_PhysicsUnits.globalCollisionTolerance ); + solver.DoSweep(); + if ( tr.fraction < ptr->fraction ) + { + *ptr = tr; + } + } + ptr->endpos = start*(1.f-ptr->fraction) + end * ptr->fraction; + if ( ptr->DidHit() ) + { + ptr->plane.dist = DotProduct( ptr->endpos, ptr->plane.normal ); + } +} + + +static void CalculateSeparatingPlane( trace_t *ptr, ITraceObject *sweepObject, CTraceRay *ray, ITraceObject *obstacle, simplex_t &simplex ); + + +//----------------------------------------------------------------------------- +// What is this doing? It's going to be hard to understand without reading a +// reference on the GJK algorithm. But here's a quick overview: +// Basically (remember this is glossing over a ton of details!) the +// algorithm is building up a simplex that is trying to contain the origin. +// A simplex is a point, line segment, triangle, or tetrahedron - depending on +// how many verts you have. +// Anyway it slowly builds one of these one vert at a time with a directed search. +// So you start out with a point, then it guesses the next point that would be +// most likely to form a line through the origin. If the line doesn't go quite +// through the origin it tries to find a third point to capture the origin +// within a triangle. If that doesn't work it tries to make a +// tent (tetrahedron) out of the triangle to capture the origin. +// +// But at each step if the origin is not contained within, it tries to +// find which sub-feature is most likely to be in the solution. In +// the point case it's always just the point. In the line/edge case it +// can reduce back to a point (origin is closest to one of the points) +// or be the line (origin is closest to some point between them). +// Same with the triangle (origin is closest to one vert - vert, origin is +// closest to one edge - reduce to that edge, origin is closes to some point +// in the triangle's surface - keep the whole triangle). With a tetrahedron +// keeping the whole isn't possible unless the origin is inside and you're +// done (the origin has been captured). +// +// "You're done" means that there is an intersection between the two +// volumes. Assuming you're testing a sweep, it still has to test whether that +// sweep can be shrunk back until there is no intersection. So it checks that. +// If it's a swept test so it does the search with SolveMeshIntersection +// Otherwise, there's nothing to shrink, so you set startsolid and allsolid +// because it's a point/box in solid test, not a swept box/ray hits solid test. +// +// Why is it trying to capture the origin? Basically GJK sets up a space +// and a convex hull (the minkowski sum) in that space. The convex hull +// represents a field of the distances between different features of the pair +// of objects (e.g. for two circles, this minkowski sum is just a circle). +// So the origin is the point in the field where the distance between the +// objects is zero. This means they intersect. +//----------------------------------------------------------------------------- +#if defined(_X360) +inline void VectorNormalize_FastLowPrecision( Vector &a ) +{ + float quad = (a.x*a.x) + (a.y*a.y) + (a.z*a.z); + float ilen = __frsqrte(quad); + a.x *= ilen; + a.y *= ilen; + a.z *= ilen; +} +#else +#define VectorNormalize_FastLowPrecision VectorNormalize +#endif + +bool CTraceSolver::SweepSingleConvex( void ) +{ + VPROF("TraceSolver::SweepSingleConvex"); + simplex_t simplex; + simplexvert_t vert; + Vector tmp; + + simplex.vertCount = 0; + + if ( m_pointClosestToIntersection == vec3_origin ) + { + m_pointClosestToIntersection.Init(1,0,0); + } + + float testLen = 1; + Vector dir = -m_pointClosestToIntersection; + VectorNormalize_FastLowPrecision(dir); + // safe loop, max 100 iterations + for ( int i = 0; i < 100; i++ ) + { + // map the direction into the minkowski sum, get a new surface point + vert.testIndex = m_sweepObject->SupportMap( dir, &vert.position ); + vert.sweepIndex = m_ray->SupportMap( dir, &tmp ); + VectorAdd( vert.position, tmp, vert.position ); + vert.obstacleIndex = m_obstacle->SupportMap( -dir, &tmp ); + VectorSubtract( vert.position, tmp, vert.position ); + + testLen = DotProduct( dir, vert.position ); + // found a separating axis, no intersection + if ( testLen < 0 ) + { + VPROF("SolveSeparation"); + // make sure we're separated by at least m_epsilon + testLen = fabs(testLen); + if ( testLen < m_epsilon && m_ray->m_length > 0 ) + { + // not separated by enough + Vector normal = dir; + if ( testLen > 0 ) + { + // try to find a better separating plane or clip the ray to the current one + for ( int j = 0; j < 20; j++ ) + { + Vector lastVert = vert.position; + simplex.SolveGJKSet( vert, &m_pointClosestToIntersection ); + dir = -m_pointClosestToIntersection; + VectorNormalize_FastLowPrecision( dir ); + // map the direction into the minkowski sum, get a new surface point + vert.testIndex = m_sweepObject->SupportMap( dir, &vert.position ); + vert.sweepIndex = m_ray->SupportMap( dir, &tmp ); + VectorAdd( vert.position, tmp, vert.position ); + vert.obstacleIndex = m_obstacle->SupportMap( -dir, &tmp ); + VectorSubtract( vert.position, tmp, vert.position ); + + // found a separating axis, no intersection + float est = -DotProduct( dir, vert.position ); + if ( est > m_epsilon ) // big enough separation, no hit + return false; + // take plane with the most separation + if ( est > testLen ) + { + testLen = est; + normal = dir; + } + float last = -DotProduct( dir, lastVert ); + // search is not converging, exit. + if ( (est - last) > -1e-4f ) + break; + } + } + + // This trace is going to miss, but not by enough. + // Hit the separating plane instead + float dot = -DotProduct( m_ray->m_delta, normal ); + if ( dot < -(m_epsilon*0.1) || (dot < -1e-4f && testLen < (m_epsilon*0.9)) ) + { + CM_ClearTrace( &m_trace ); + float backupDistance = m_epsilon - testLen; + backupDistance = -(backupDistance * m_ray->m_baseLength) / dot; + m_traceLength = m_ray->m_length - backupDistance; + if ( m_traceLength < 0 ) + { + m_traceLength = 0; + // try sliding along the surface of the minkowski sum + backupDistance = SolveMeshIntersection2D( simplex ); + if ( m_ray->m_length > backupDistance ) + { + m_traceLength = m_ray->m_length - backupDistance; + } + } + m_trace.plane.normal = -normal; + // this is fixed up by the outer code + //m_trace.endpos = m_ray->m_start*(1.f-m_trace.fraction) + m_ray->m_end*m_trace.fraction; + m_trace.contents = CONTENTS_SOLID; + return true; + } + } + return false; + } + + // contains the origin + if ( simplex.SolveGJKSet( vert, &m_pointClosestToIntersection ) ) + { + VPROF("TraceSolver::SolveMeshIntersection"); + CM_ClearTrace( &m_trace ); + // now solve for t along the sweep + if ( m_ray->m_length != 0 ) + { + float dist = SolveMeshIntersection( simplex ); + if ( dist < m_ray->m_length && dist > 0.f ) + { + m_traceLength = (m_ray->m_length - dist); + CalculateSeparatingPlane( &m_trace, m_sweepObject, m_ray, m_obstacle, simplex ); + float dot = DotProduct( m_ray->m_dir, m_trace.plane.normal ); + if ( dot < 0 ) + { + m_traceLength += (m_epsilon / dot); + } + + if ( m_traceLength < 0 ) + { + m_traceLength = 0; + } + //m_trace.fraction = m_traceLength * m_ray->m_ooBaseLength; + //m_trace.endpos = m_ray->m_start*(1.f-m_trace.fraction) + m_ray->m_end*m_trace.fraction; + m_trace.contents = CONTENTS_SOLID; + } + else + { + // UNDONE: This case happens when you start solid as well as when a false + // intersection is detected at the very end of the trace + m_trace.startsolid = true; + m_trace.allsolid = true; + m_traceLength = 0; + } + } + else + { + m_trace.startsolid = true; + m_trace.allsolid = true; + m_traceLength = 0; + } + return true; + } + dir = -m_pointClosestToIntersection; + VectorNormalize_FastLowPrecision( dir ); + } + + // BUGBUG: The solution never converged - something is probably wrong! + AssertMsg( false, "Solution never converged."); + return false; +} + + +// NEW SWEPT GJK SOLVER 2/16/2006 +// convenience routines - just makes the code a little simpler. +FORCEINLINE bool simplex_t::TriangleSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &faceNormal, Vector *pOut ) +{ + vertCount = 3; + verts[outIndex] = newPoint; + *pOut = -faceNormal; + return false; +} +FORCEINLINE bool simplex_t::EdgeSimplex( const simplexvert_t &newPoint, int outIndex, const Vector &edge, Vector *pOut ) +{ + vertCount = 2; + verts[outIndex] = newPoint; + Vector cross; + CrossProduct( edge, newPoint.position, cross ); + CrossProduct( cross, edge, *pOut ); + return false; +} + +FORCEINLINE bool simplex_t::PointSimplex( const simplexvert_t &newPoint, Vector *pOut ) +{ + vertCount = 1; + verts[0] = newPoint; + *pOut = newPoint.position; + return false; +} + +// In general the voronoi region routines have comments referring to the verts +// All of the code assumes that vert A is the new vert being added to the set +// verts B, C, D are the previous set. If BCD is a triangle it is assumed to be +// counter-clockwise winding order. This must be maintained by the code! + +// parametric value for closes point on a line segment (p0->p1) to the origin. +bool simplex_t::SolveVoronoiRegion2( const simplexvert_t &newPoint, Vector *pOut ) +{ + // solve the line segment AB (where A is the new point) + Vector AB = verts[0].position - newPoint.position; + float d = DotProduct(AB, newPoint.position); + if ( d < 0 ) + { + return EdgeSimplex(newPoint, 1, AB, pOut); + } + else + { + return PointSimplex(newPoint, pOut); + } +} + +// UNDONE: Collapse these routines into a single general routine? +bool simplex_t::SolveVoronoiRegion3( const simplexvert_t &newPoint, Vector *pOut ) +{ + // solve the triangle ABC (where A is the new point) + Vector AB = verts[0].position - newPoint.position; + Vector AC = verts[1].position - newPoint.position; + Vector ABC; + CrossProduct(AB, AC, ABC); + Vector ABCxAC; + CrossProduct(ABC, AC, ABCxAC); + float d = DotProduct(ABCxAC, newPoint.position); + // edge AC or edgeAB / A? + if ( d < 0 ) + { + d = DotProduct(AC, newPoint.position); + if ( d < 0 ) + { + // edge AC + return EdgeSimplex(newPoint, 0, AC, pOut); + } + } + else + { + // face or A / edge AB? + Vector ABxABC; + CrossProduct(AB, ABC, ABxABC); + d = DotProduct(ABxABC, newPoint.position); + if ( d > 0 ) + { + // closest to face + vertCount = 3; + d = DotProduct(ABC, newPoint.position); + // in front of face, return opposite direction + if ( d < 0 ) + { + verts[2] = newPoint; + *pOut = -ABC; + return false; + } + verts[2] = verts[1]; // swap to keep CCW + verts[1] = newPoint; + *pOut = ABC; + return false; + } + } + // edge AB or A + d = DotProduct(AB, newPoint.position); + if ( d < 0 ) + { + return EdgeSimplex(newPoint, 1, AB, pOut); + } + return PointSimplex(newPoint, pOut); +} + +bool simplex_t::SolveVoronoiRegion4( const simplexvert_t &newPoint, Vector *pOut ) +{ + // solve the tetrahedron ABCD (where A is the new point) + + // compute edge vectors + Vector AB = verts[0].position - newPoint.position; + Vector AC = verts[1].position - newPoint.position; + Vector AD = verts[2].position - newPoint.position; + + // compute face normals + Vector ABC, ACD, ADB; + CrossProduct( AB, AC, ABC ); + CrossProduct( AC, AD, ACD ); + CrossProduct( AD, AB, ADB ); + + // edge plane normals + Vector ABCxAC, ABxABC; + CrossProduct( ABC, AC, ABCxAC ); + CrossProduct( AB, ABC, ABxABC ); + Vector ACDxAD, ACxACD; + CrossProduct( ACD, AD, ACDxAD ); + CrossProduct( AC, ACD, ACxACD ); + Vector ADBxAB, ADxADB; + CrossProduct( ADB, AB, ADBxAB ); + CrossProduct( AD, ADB, ADxADB ); + + int faceFlags = 0; + float d; + d = DotProduct(ABC,newPoint.position); + if ( d < 0 ) + { + faceFlags |= 0x1; + } + d = DotProduct(ACD,newPoint.position); + if ( d < 0 ) + { + faceFlags |= 0x2; + } + d = DotProduct(ADB,newPoint.position); + if ( d < 0 ) + { + faceFlags |= 0x4; + } + + switch( faceFlags ) + { + case 0: + // inside all 3 faces, we're done + verts[3] = newPoint; + vertCount = 4; + return true; + case 1: + // ABC only, solve as a triangle + return SolveVoronoiRegion3(newPoint, pOut); + case 2: + // ACD only, solve as a triangle + verts[0] = verts[2]; // collapse BCD to DC + return SolveVoronoiRegion3(newPoint, pOut); + case 4: + // ADB only, solve as a triangle + verts[1] = verts[2]; // collapse BCD to BD + return SolveVoronoiRegion3(newPoint, pOut); + case 3: + { + // in front of ABC & ACD + d = DotProduct(ABCxAC, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(ACxACD, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(AC,newPoint.position); + if ( d < 0 ) + { + // edge AC + return EdgeSimplex( newPoint, 0, AC, pOut); + } + // point A + return PointSimplex(newPoint, pOut); + } + else + { + d = DotProduct(ACDxAD, newPoint.position); + if ( d < 0 ) + { + // edge AD + verts[0] = verts[2]; // collapse BCD to D + return EdgeSimplex(newPoint, 1, AD, pOut); + } + // face ACD + return TriangleSimplex(newPoint,0,ACD, pOut); + } + } + else + { + d = DotProduct(ABxABC, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(AB, newPoint.position); + if ( d < 0 ) + { + // edge AB + return EdgeSimplex(newPoint, 1, AB, pOut); + } + return PointSimplex(newPoint, pOut); + } + return TriangleSimplex(newPoint,2,ABC,pOut); + } + } + break; + case 5: + { + // in front of ADB & ABC + d = DotProduct(ADBxAB, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(ABxABC, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(AB,newPoint.position); + if ( d < 0 ) + { + // edge AB + return EdgeSimplex( newPoint, 1, AB , pOut); + } + // point A + return PointSimplex(newPoint, pOut); + } + else + { + d = DotProduct(ABCxAC, newPoint.position); + if ( d < 0 ) + { + // edge AC + return EdgeSimplex(newPoint, 0, AC, pOut); + } + // face ABC + return TriangleSimplex(newPoint,2,ABC,pOut); + } + } + else + { + d = DotProduct(ADxADB, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(AD, newPoint.position); + if ( d < 0 ) + { + // edge AD + verts[0] = verts[2]; // collapse BCD to D + return EdgeSimplex(newPoint, 1, AD, pOut); + } + return PointSimplex(newPoint, pOut); + } + return TriangleSimplex(newPoint,1,ADB,pOut); + } + } + break; + case 6: + { + // in front of ACD & ADB + d = DotProduct(ACDxAD, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(ADxADB, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(AD,newPoint.position); + if ( d < 0 ) + { + // edge AD + verts[0] = verts[2]; // collapse BCD to D + return EdgeSimplex(newPoint, 1, AD, pOut); + } + // point A + return PointSimplex(newPoint, pOut); + } + else + { + d = DotProduct(ADBxAB, newPoint.position); + if ( d < 0 ) + { + // edge AB + return EdgeSimplex(newPoint, 1, AB, pOut); + } + // face ADB + return TriangleSimplex(newPoint,1,ADB, pOut); + } + } + else + { + d = DotProduct(ACxACD, newPoint.position); + if ( d < 0 ) + { + d = DotProduct(AC, newPoint.position); + if ( d < 0 ) + { + // edge AC + return EdgeSimplex(newPoint, 0, AC, pOut); + } + return PointSimplex(newPoint, pOut); + } + return TriangleSimplex(newPoint,0,ACD, pOut); + } + } + break; + case 7: + { + d = DotProduct(AB, newPoint.position); + if ( d < 0 ) + { + return EdgeSimplex(newPoint, 1, AB, pOut); + } + else + { + d = DotProduct(AC, newPoint.position); + if ( d < 0 ) + { + return EdgeSimplex(newPoint, 0, AC, pOut); + } + else + { + d = DotProduct(AD, newPoint.position); + if ( d < 0 ) + { + verts[0] = verts[2]; // collapse BCD to D + return EdgeSimplex(newPoint, 1, AD, pOut); + } + return PointSimplex(newPoint, pOut); + } + } + } + } + verts[3] = newPoint; + vertCount = 4; + return true; +} + + +bool simplex_t::SolveGJKSet( const simplexvert_t &w, Vector *pOut ) +{ + VPROF("TraceSolver::simplex::SolveGJKSet"); + +#if 0 + for ( int v = 0; v < vertCount; v++ ) + { + for ( int v2 = v+1; v2 < vertCount; v2++ ) + { + if ( (verts[v].obstacleIndex == verts[v2].obstacleIndex) && + (verts[v].sweepIndex == verts[v2].sweepIndex) && + (verts[v].testIndex == verts[v2].testIndex) ) + { + // same vert in the list twice! degenerate + Assert(0); + } + } + } +#endif + + switch( vertCount ) + { + case 0: + vertCount = 1; + verts[0] = w; + *pOut = w.position; + return false; + case 1: + return SolveVoronoiRegion2( w, pOut ); + case 2: + return SolveVoronoiRegion3( w, pOut ); + case 3: + return SolveVoronoiRegion4( w, pOut ); + } + + return true; +} + + +void CalculateSeparatingPlane( trace_t *ptr, ITraceObject *sweepObject, CTraceRay *ray, ITraceObject *obstacle, simplex_t &simplex ) +{ + int testCount = 1, obstacleCount = 1; + unsigned int testIndex[4], obstacleIndex[4]; + testIndex[0] = simplex.verts[0].testIndex; + obstacleIndex[0] = simplex.verts[0].obstacleIndex; + Assert( simplex.vertCount <= 4 ); + int i, j; + for ( i = 1; i < simplex.vertCount; i++ ) + { + for ( j = 0; j < obstacleCount; j++ ) + { + if ( obstacleIndex[j] == simplex.verts[i].obstacleIndex ) + break; + } + if ( j == obstacleCount ) + { + obstacleIndex[obstacleCount++] = simplex.verts[i].obstacleIndex; + } + + for ( j = 0; j < testCount; j++ ) + { + if ( testIndex[j] == simplex.verts[i].testIndex ) + break; + } + if ( j == testCount ) + { + testIndex[testCount++] = simplex.verts[i].testIndex; + } + } + + if ( simplex.vertCount < 3 ) + { + if ( simplex.vertCount == 2 && testCount == 2 ) + { + // edge / point + Vector t0 = sweepObject->GetVertByIndex( testIndex[0] ); + Vector t1 = sweepObject->GetVertByIndex( testIndex[1] ); + Vector edge = t1-t0; + Vector tangent = CrossProduct( edge, ray->m_delta ); + ptr->plane.normal = CrossProduct( edge, tangent ); + VectorNormalize( ptr->plane.normal ); + ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal ); + return; + } + } + if ( testCount == 3 ) + { + // face / xxx + Vector t0 = sweepObject->GetVertByIndex( testIndex[0] ); + Vector t1 = sweepObject->GetVertByIndex( testIndex[1] ); + Vector t2 = sweepObject->GetVertByIndex( testIndex[2] ); + ptr->plane.normal = CrossProduct( t1-t0, t2-t0 ); + VectorNormalize( ptr->plane.normal ); + if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 ) + { + ptr->plane.normal = -ptr->plane.normal; + } + ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal ); + } + else if ( testCount == 2 && obstacleCount == 2 ) + { + // edge / edge + Vector t0 = sweepObject->GetVertByIndex( testIndex[0] ); + Vector t1 = sweepObject->GetVertByIndex( testIndex[1] ); + Vector t2 = obstacle->GetVertByIndex( obstacleIndex[0] ); + Vector t3 = obstacle->GetVertByIndex( obstacleIndex[1] ); + ptr->plane.normal = CrossProduct( t1-t0, t3-t2 ); + VectorNormalize( ptr->plane.normal ); + if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 ) + { + ptr->plane.normal = -ptr->plane.normal; + } + ptr->plane.dist = DotProduct( t0 + ptr->endpos, ptr->plane.normal ); + } + else if ( obstacleCount == 3 ) + { + // xxx / face + Vector t0 = obstacle->GetVertByIndex( obstacleIndex[0] ); + Vector t1 = obstacle->GetVertByIndex( obstacleIndex[1] ); + Vector t2 = obstacle->GetVertByIndex( obstacleIndex[2] ); + ptr->plane.normal = CrossProduct( t1-t0, t2-t0 ); + VectorNormalize( ptr->plane.normal ); + if ( DotProduct( ptr->plane.normal, ray->m_delta ) > 0 ) + { + ptr->plane.normal = -ptr->plane.normal; + } + ptr->plane.dist = DotProduct( t0, ptr->plane.normal ); + } + else + { + ptr->plane.normal = -ray->m_dir; + if ( simplex.vertCount ) + { + ptr->plane.dist = DotProduct( ptr->plane.normal, obstacle->GetVertByIndex( simplex.verts[0].obstacleIndex ) ); + } + else + ptr->plane.dist = 0.f; + } +} + +// clip a direction vector to a plane (ray begins at the origin) +inline float Clip( const Vector &dir, const Vector &pos, const Vector &normal ) +{ + float dist = DotProduct(pos, normal); + float cosTheta = DotProduct(dir,normal); + if ( cosTheta > 0 ) + return dist / cosTheta; + + // parallel or not facing the plane + return 1e24f; +} + +// This is the first iteration of solving time of intersection. +// It needs to test all 4 faces of the tetrahedron to find the one the ray leaves through +// this is done by finding the closest plane by clipping the ray to each plane +Vector simplex_t::ClipRayToTetrahedronBase( const Vector &dir ) +{ + Vector AB = verts[0].position - verts[3].position; + Vector AC = verts[1].position - verts[3].position; + Vector AD = verts[2].position - verts[3].position; + Vector BC = verts[1].position - verts[0].position; + Vector BD = verts[2].position - verts[0].position; + + // compute face normals + Vector ABC, ACD, ADB, BCD; + CrossProduct( AB, AC, ABC ); + CrossProduct( AC, AD, ACD ); + CrossProduct( AD, AB, ADB ); + CrossProduct( BD, BC, BCD ); // flipped to point out of the tetrahedron + + // NOTE: These cancel out in the clipping equation + //VectorNormalize(ABC); + //VectorNormalize(ACD); + //VectorNormalize(ADB); + //VectorNormalize(BCD); + + // Assert valid tetrahedron/winding order +#if CHECK_TOI_CALCS + Assert(DotProduct(verts[2].position, ABC) < DotProduct(verts[3].position, ABC )); + Assert(DotProduct(verts[0].position, ACD) < DotProduct(verts[3].position, ACD )); + Assert(DotProduct(verts[1].position, ADB) < DotProduct(verts[3].position, ADB )); + Assert(DotProduct(verts[3].position, BCD) < DotProduct(verts[0].position, BCD )); +#endif + + float dists[4]; + dists[0] = Clip( dir, verts[3].position, ABC ); + dists[1] = Clip( dir, verts[3].position, ACD ); + dists[2] = Clip( dir, verts[3].position, ADB ); + dists[3] = Clip( dir, verts[0].position, BCD ); + float dmin = dists[3]; + int best = 3; + for ( int i = 0; i < 3; i++ ) + { + if ( dists[i] < dmin ) + { + best = i; + dmin = dists[i]; + } + } + + vertCount = 3; + // push back down to a triangle + // Note that we need to preserve winding so that the vector order assumptions above are still valid! + switch( best ) + { + case 0: + verts[2] = verts[3]; + return ABC; + case 1: + verts[0] = verts[3]; + return ACD; + case 2: + verts[1] = verts[3]; + return ADB; + case 3: + // swap 1 & 2 + verts[3] = verts[1]; + verts[1] = verts[2]; + verts[2] = verts[3]; + return BCD; + } + Assert(0); // failed! + return vec3_origin; +} + +// for subsequent iterations you don't need to test one of the faces (face you previously left through) +// so only test the three faces involving the new point A +Vector simplex_t::ClipRayToTetrahedron( const Vector &dir ) +{ + Vector AB = verts[0].position - verts[3].position; + Vector AC = verts[1].position - verts[3].position; + Vector AD = verts[2].position - verts[3].position; + + // compute face normals + Vector ABC, ACD, ADB; + CrossProduct( AB, AC, ABC ); + CrossProduct( AC, AD, ACD ); + CrossProduct( AD, AB, ADB ); + + // NOTE: These cancel out in the clipping equation + //VectorNormalize(ABC); + //VectorNormalize(ACD); + //VectorNormalize(ADB); + + // Assert valid tetrahedron/winding order +#if CHECK_TOI_CALCS + Assert(DotProduct(verts[2].position, ABC) < DotProduct(verts[3].position, ABC )); + Assert(DotProduct(verts[0].position, ACD) < DotProduct(verts[3].position, ACD )); + Assert(DotProduct(verts[1].position, ADB) < DotProduct(verts[3].position, ADB )); +#endif + + float dists[3]; + dists[0] = Clip( dir, verts[3].position, ABC ); + dists[1] = Clip( dir, verts[3].position, ACD ); + dists[2] = Clip( dir, verts[3].position, ADB ); + + float dmin = dists[2]; + int best = 2; + for ( int i = 0; i < 2; i++ ) + { + if ( dists[i] < dmin ) + { + best = i; + dmin = dists[i]; + } + } + + vertCount = 3; + // push back down to a triangle + // Note that we need to preserve winding so that the vector order assumptions above are still valid! + switch( best ) + { + case 0: + verts[2] = verts[3]; + return ABC; + case 1: + verts[0] = verts[3]; + return ACD; + case 2: + verts[1] = verts[3]; + return ADB; + } + return vec3_origin; +} + +float simplex_t::ClipRayToTriangle( const Vector &dir, float epsilon ) +{ + Vector AB = verts[0].position - verts[2].position; + Vector AC = verts[1].position - verts[2].position; + Vector BC = verts[1].position - verts[0].position; + + // compute face normal + Vector ABC; + CrossProduct( AB, AC, ABC ); // this points toward the origin + VectorNormalize(ABC); + Vector edgeAB, edgeAC, edgeBC; + // these should point out of the triangle + CrossProduct( AB, ABC, edgeAB ); + CrossProduct( ABC, AC, edgeAC ); + CrossProduct( BC, ABC, edgeBC ); + + // NOTE: These cancel out in the clipping equation + VectorNormalize(edgeAB); + VectorNormalize(edgeAC); + VectorNormalize(edgeBC); + +#if CHECK_TOI_CALCS + Assert(DotProduct(verts[2].position, ABC) < 0); // points toward + // Assert valid triangle/winding order (all normals point away from the origin) + Assert(DotProduct(verts[2].position, edgeAB) > 0); + Assert(DotProduct(verts[2].position, edgeAC) > 0); + Assert(DotProduct(verts[0].position, edgeBC) > 0); +#endif + + float dists[3]; + dists[0] = Clip( dir, verts[0].position, edgeAB ); + dists[1] = Clip( dir, verts[1].position, edgeAC ); + dists[2] = Clip( dir, verts[1].position, edgeBC ); + Vector *normals[3] = {&edgeAB, &edgeAC, &edgeBC}; + + float dmin = dists[0]; + int best = 0; + if ( dists[1] < dmin ) + { + dmin = dists[1]; + best = 1; + } + if ( dists[2] < dmin ) + { + best = 2; + dmin = dists[2]; + } + float dot = DotProduct( dir, *normals[best] ); + if ( dot <= 0 ) + return 1e24f; + dmin += epsilon/dot; + + return dmin; +} + +// Solve for time of intersection along the sweep +// Do this by iteratively clipping the ray to the tetrahedron containing the origin +// when a triangle is found intersecting the ray, reduce the simplex to that triangle +// and then re-expand it to a tetrahedron using the support point normal to the triangle (away from the origin) +// iterate until no new points can be found. That's the surface of the sum. +float CTraceSolver::SolveMeshIntersection( simplex_t &simplex ) +{ + Vector tmp; + Assert( simplex.vertCount == 4 ); + if ( simplex.vertCount < 4 ) + return 0.0f; + + Vector v = simplex.ClipRayToTetrahedronBase( m_ray->m_dir ); + + simplexvert_t vert; + // safe loop, max 100 iterations + for ( int i = 0; i < 100; i++ ) + { + VectorNormalize(v); + vert.testIndex = m_sweepObject->SupportMap( v, &vert.position ); + vert.sweepIndex = m_ray->SupportMap( v, &tmp ); + VectorAdd( vert.position, tmp, vert.position ); + vert.obstacleIndex = m_obstacle->SupportMap( -v, &tmp ); + VectorSubtract( vert.position, tmp, vert.position ); + + // map the new separating axis (NOTE: This test is inverted from the GJK - we are trying to get out, not in) + // found a separating axis, we've moved the sweep back enough + float dist = DotProduct( v, simplex.verts[0].position ) + TEST_EPSILON; + if ( DotProduct( v, vert.position ) <= dist ) + { + Vector BC = simplex.verts[1].position - simplex.verts[0].position; + Vector BD = simplex.verts[2].position - simplex.verts[0].position; + + // compute face normals + Vector BCD; + CrossProduct( BC, BD, BCD ); + // NOTE: This cancels out inside Clip() + //VectorNormalize( BCD ); + // clip ray to triangle + return Clip( m_ray->m_dir, simplex.verts[0].position, BCD ); + } + + // add the new vert + simplex.verts[simplex.vertCount] = vert; + simplex.vertCount++; + v = simplex.ClipRayToTetrahedron( m_ray->m_dir ); + } + + Assert(0); + return 0.0f; +} + +// similar to SolveMeshIntersection, but solves projected into the 2D triangle simplex remaining +// this is used for the near miss case +float CTraceSolver::SolveMeshIntersection2D( simplex_t &simplex ) +{ + AssertMsg( simplex.vertCount == 3, "simplex.vertCount != 3: %d", simplex.vertCount ); + if ( simplex.vertCount != 3 ) + return 0.0f; + + // note: This should really do this iteratively in case the triangle is coplanar with another triangle that + // is between this one and the edge of the sum in this plane + float dist = simplex.ClipRayToTriangle( m_ray->m_dir, m_epsilon ); + return dist; +} + + +static const Vector g_xpos(1,0,0), g_xneg(-1,0,0); +static const Vector g_ypos(0,1,0), g_yneg(0,-1,0); +static const Vector g_zpos(0,0,1), g_zneg(0,0,-1); + +void TraceGetAABB_r( Vector *pMins, Vector *pMaxs, const IVP_Compact_Ledgetree_Node *node, CTraceIVP &ivp ) +{ + if ( node->is_terminal() == IVP_TRUE ) + { + Vector tmp; + ivp.SetLedge( node->get_compact_ledge() ); + ivp.SupportMap( g_xneg, &tmp ); + AddPointToBounds( tmp, *pMins, *pMaxs ); + ivp.SupportMap( g_yneg, &tmp ); + AddPointToBounds( tmp, *pMins, *pMaxs ); + ivp.SupportMap( g_zneg, &tmp ); + AddPointToBounds( tmp, *pMins, *pMaxs ); + ivp.SupportMap( g_xpos, &tmp ); + AddPointToBounds( tmp, *pMins, *pMaxs ); + ivp.SupportMap( g_ypos, &tmp ); + AddPointToBounds( tmp, *pMins, *pMaxs ); + ivp.SupportMap( g_zpos, &tmp ); + AddPointToBounds( tmp, *pMins, *pMaxs ); + return; + } + + TraceGetAABB_r( pMins, pMaxs, node->left_son(), ivp ); + TraceGetAABB_r( pMins, pMaxs, node->right_son(), ivp ); +} + +void CPhysicsTrace::GetAABB( Vector *pMins, Vector *pMaxs, const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles ) +{ + CTraceIVP ivp( pCollide, collideOrigin, collideAngles ); + + if ( ivp.SetSingleConvex() ) + { + Vector tmp; + ivp.SupportMap( g_xneg, &tmp ); + pMins->x = tmp.x; + ivp.SupportMap( g_yneg, &tmp ); + pMins->y = tmp.y; + ivp.SupportMap( g_zneg, &tmp ); + pMins->z = tmp.z; + + ivp.SupportMap( g_xpos, &tmp ); + pMaxs->x = tmp.x; + ivp.SupportMap( g_ypos, &tmp ); + pMaxs->y = tmp.y; + ivp.SupportMap( g_zpos, &tmp ); + pMaxs->z = tmp.z; + } + else + { + const IVP_Compact_Ledgetree_Node *lt_node_root; + lt_node_root = pCollide->GetCompactSurface()->get_compact_ledge_tree_root(); + ClearBounds( *pMins, *pMaxs ); + TraceGetAABB_r( pMins, pMaxs, lt_node_root, ivp ); + } + // JAY: Disable this here, do it in the engine instead. That way the tools get + // accurate bboxes +#if 0 + const float radius = g_PhysicsUnits.collisionSweepEpsilon; + mins -= Vector(radius,radius,radius); + maxs += Vector(radius,radius,radius); +#endif +} + +void TraceGetExtent_r( const IVP_Compact_Ledgetree_Node *node, CTraceIVP &ivp, const Vector &dir, float &dot, Vector &point ) +{ + if ( node->is_terminal() == IVP_TRUE ) + { + ivp.SetLedge( node->get_compact_ledge() ); + Vector tmp; + ivp.SupportMap( dir, &tmp ); + float newDot = DotProduct( tmp, dir ); + + if ( newDot > dot ) + { + dot = newDot; + point = tmp; + } + return; + } + + TraceGetExtent_r( node->left_son(), ivp, dir, dot, point ); + TraceGetExtent_r( node->right_son(), ivp, dir, dot, point ); +} + + +Vector CPhysicsTrace::GetExtent( const CPhysCollide *pCollide, const Vector &collideOrigin, const QAngle &collideAngles, const Vector &direction ) +{ + CTraceIVP ivp( pCollide, collideOrigin, collideAngles ); + + if ( ivp.SetSingleConvex() ) + { + Vector tmp; + ivp.SupportMap( direction, &tmp ); + return tmp; + } + else + { + const IVP_Compact_Ledgetree_Node *lt_node_root; + lt_node_root = pCollide->GetCompactSurface()->get_compact_ledge_tree_root(); + Vector out = vec3_origin; + float tmp = -1e6f; + TraceGetExtent_r( lt_node_root, ivp, direction, tmp, out ); + return out; + } +} + +bool CPhysicsTrace::IsBoxIntersectingCone( const Vector &boxAbsMins, const Vector &boxAbsMaxs, const truncatedcone_t &cone ) +{ + trace_t tr; + CM_ClearTrace( &tr ); + bool bPoint = (boxAbsMins == boxAbsMaxs) ? true : false; + CTraceAABB box( boxAbsMins - cone.origin, boxAbsMaxs - cone.origin, bPoint ); + CTraceCone traceCone( cone, -cone.origin ); + + // offset the space of this sweep so that the surface is at the origin of the solution space + CTraceRay ray( vec3_origin, vec3_origin ); + + CTraceSolver solver( &tr, &box, &ray, &traceCone, boxAbsMaxs ); + solver.DoSweep(); + + return tr.startsolid; +} + + + +CPhysicsTrace::CPhysicsTrace() +{ +} + +CPhysicsTrace::~CPhysicsTrace() +{ +} + +CVisitHash::CVisitHash() +{ + m_vertVisitID = 1; + memset( m_vertVisit, 0, sizeof(m_vertVisit) ); +} diff --git a/vphysics/traceperf/stdafx.cpp b/vphysics/traceperf/stdafx.cpp new file mode 100644 index 0000000..d0d4a7c --- /dev/null +++ b/vphysics/traceperf/stdafx.cpp @@ -0,0 +1,9 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// stdafx.cpp : source file that includes just the standard includes +// traceperf.pch will be the pre-compiled header +// stdafx.obj will contain the pre-compiled type information + +#include "stdafx.h" + +// TODO: reference any additional headers you need in STDAFX.H +// and not in this file diff --git a/vphysics/traceperf/stdafx.h b/vphysics/traceperf/stdafx.h new file mode 100644 index 0000000..bacc26d --- /dev/null +++ b/vphysics/traceperf/stdafx.h @@ -0,0 +1,15 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// stdafx.h : include file for standard system include files, +// or project specific include files that are used frequently, but +// are changed infrequently +// + +#pragma once + + +#include "phyfile.h" +#include "vphysics_interface.h" +#include "tier0/icommandline.h" +#include "tier0/vprof.h" + +// TODO: reference additional headers your program requires here diff --git a/vphysics/traceperf/traceperf.cpp b/vphysics/traceperf/traceperf.cpp new file mode 100644 index 0000000..9d0a705 --- /dev/null +++ b/vphysics/traceperf/traceperf.cpp @@ -0,0 +1,406 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// traceperf.cpp : Defines the entry point for the console application. +// + +#include "stdafx.h" +#include "gametrace.h" +#include "fmtstr.h" +#include "appframework/appframework.h" +#include "filesystem.h" +#include "filesystem_init.h" +#include "tier1/tier1.h" +#include "tier2/tier2.h" +#include "tier3/tier3.h" + +IPhysicsCollision *physcollision = NULL; + +#define NUM_COLLISION_TESTS 2500 + +void ReadPHYFile(const char *name, vcollide_t &collide ) +{ + FileHandle_t fp = g_pFullFileSystem->Open(name, "rb"); + if (!fp) + Error ("Couldn't open %s", name); + + phyheader_t header; + + g_pFullFileSystem->Read( &header, sizeof(header), fp ); + if ( header.size != sizeof(header) || header.solidCount <= 0 ) + return; + + int fileSize = g_pFullFileSystem->Size(fp); + + char *buf = (char *)_alloca( fileSize ); + g_pFullFileSystem->Read( buf, fileSize, fp ); + g_pFullFileSystem->Close( fp ); + + physcollision->VCollideLoad( &collide, header.solidCount, (const char *)buf, fileSize ); +} + + +struct testlist_t +{ + Vector start; + Vector end; + Vector normal; + bool hit; +}; + +struct benchresults_t +{ + int collisionTests; + int collisionHits; + float totalTime; + float rayTime; + float boxTime; +}; + +testlist_t g_Traces[NUM_COLLISION_TESTS]; +void Benchmark_PHY( const CPhysCollide *pCollide, benchresults_t *pOut ) +{ + int i; + Vector start = vec3_origin; + static Vector *targets = NULL; + static bool first = true; + static float test[2] = {1,1}; + if ( first ) + { + float radius = 0; + float theta = 0; + float phi = 0; + for ( int i = 0; i < NUM_COLLISION_TESTS; i++ ) + { + radius += NUM_COLLISION_TESTS * 123.123f; + radius = fabs(fmod(radius, 128)); + theta += NUM_COLLISION_TESTS * 0.76f; + theta = fabs(fmod(theta, DEG2RAD(360))); + phi += NUM_COLLISION_TESTS * 0.16666666f; + phi = fabs(fmod(phi, DEG2RAD(180))); + + float st, ct, sp, cp; + SinCos( theta, &st, &ct ); + SinCos( phi, &sp, &cp ); + st = sin(theta); + ct = cos(theta); + sp = sin(phi); + cp = cos(phi); + + g_Traces[i].start.x = radius * ct * sp; + g_Traces[i].start.y = radius * st * sp; + g_Traces[i].start.z = radius * cp; + } + first = false; + } + + float duration = 0; + Vector size[2]; + size[0].Init(0,0,0); + size[1].Init(16,16,16); + +#if VPROF_LEVEL > 0 + g_VProfCurrentProfile.Reset(); + g_VProfCurrentProfile.ResetPeaks(); + g_VProfCurrentProfile.Start(); +#endif + +#if TEST_BBOX + Vector mins, maxs; + physcollision->CollideGetAABB( &mins, &maxs, pCollide, Vector(-500, 200, -100), vec3_angle ); + Vector extents = maxs - mins; + Vector center = 0.5f * (maxs+mins); + Msg("bbox: %.2f,%.2f, %.2f @ %.2f, %.2f, %.2f\n", extents.x, extents.y, extents.z, center.x, center.y, center.z ); +#endif + unsigned int hitCount = 0; + double startTime = Plat_FloatTime(); + trace_t tr; + for ( i = 0; i < NUM_COLLISION_TESTS; i++ ) + { + physcollision->TraceBox( g_Traces[i].start, start, -size[0], size[0], pCollide, vec3_origin, vec3_angle, &tr ); + if ( tr.DidHit() ) + { + g_Traces[i].end = tr.endpos; + g_Traces[i].normal = tr.plane.normal; + g_Traces[i].hit = true; + hitCount++; + } + else + { + g_Traces[i].hit = false; + } + } + for ( i = 0; i < NUM_COLLISION_TESTS; i++ ) + { + physcollision->TraceBox( g_Traces[i].start, start, -size[1], size[1], pCollide, vec3_origin, vec3_angle, &tr ); + } + duration = Plat_FloatTime() - startTime; + +#if VPROF_LEVEL > 0 + g_VProfCurrentProfile.MarkFrame(); + g_VProfCurrentProfile.Stop(); + g_VProfCurrentProfile.Reset(); + g_VProfCurrentProfile.ResetPeaks(); + g_VProfCurrentProfile.Start(); +#endif + hitCount = 0; + startTime = Plat_FloatTime(); + for ( i = 0; i < NUM_COLLISION_TESTS; i++ ) + { + physcollision->TraceBox( g_Traces[i].start, start, -size[0], size[0], pCollide, vec3_origin, vec3_angle, &tr ); + if ( tr.DidHit() ) + { + g_Traces[i].end = tr.endpos; + g_Traces[i].normal = tr.plane.normal; + g_Traces[i].hit = true; + hitCount++; + } + else + { + g_Traces[i].hit = false; + } +#if VPROF_LEVEL > 0 + g_VProfCurrentProfile.MarkFrame(); +#endif + } + double midTime = Plat_FloatTime(); + for ( i = 0; i < NUM_COLLISION_TESTS; i++ ) + { + physcollision->TraceBox( g_Traces[i].start, start, -size[1], size[1], pCollide, vec3_origin, vec3_angle, &tr ); +#if VPROF_LEVEL > 0 + g_VProfCurrentProfile.MarkFrame(); +#endif + } + double endTime = Plat_FloatTime(); + duration = endTime - startTime; + pOut->collisionTests = NUM_COLLISION_TESTS; + pOut->collisionHits = hitCount; + pOut->totalTime = duration * 1000.0f; + pOut->rayTime = (midTime - startTime) * 1000.0f; + pOut->boxTime = (endTime - midTime)*1000.0f; + +#if VPROF_LEVEL > 0 + g_VProfCurrentProfile.Stop(); + g_VProfCurrentProfile.OutputReport( VPRT_FULL & ~VPRT_HIERARCHY, NULL ); +#endif +} + +//===== Copyright � 1996-2005, Valve Corporation, All rights reserved. ======// +// +// Purpose: +// +// $NoKeywords: $ +// +//===========================================================================// + + +//----------------------------------------------------------------------------- +// The application object +//----------------------------------------------------------------------------- +class CBenchmarkApp : public CDefaultAppSystemGroup< CSteamAppSystemGroup > +{ + typedef CDefaultAppSystemGroup< CSteamAppSystemGroup > BaseClass; + +public: + // Methods of IApplication + virtual bool Create(); + virtual bool PreInit( ); + virtual int Main(); + virtual void PostShutdown(); + bool SetupSearchPaths(); + +private: + bool ParseArguments(); +}; + +DEFINE_CONSOLE_STEAM_APPLICATION_OBJECT( CBenchmarkApp ); + + +//----------------------------------------------------------------------------- +// The application object +//----------------------------------------------------------------------------- +bool CBenchmarkApp::Create() +{ + MathLib_Init( 2.2f, 2.2f, 0.0f, 2.0f, false, false, false, false ); + + // Add in the cvar factory + //AppModule_t cvarModule = LoadModule( VStdLib_GetICVarFactory() ); + //AddSystem( cvarModule, VENGINE_CVAR_INTERFACE_VERSION ); + + AppSystemInfo_t appSystems[] = + { + { "vphysics.dll", VPHYSICS_INTERFACE_VERSION }, + { "", "" } // Required to terminate the list + }; + + bool bRet = AddSystems( appSystems ); + if ( bRet ) + { + physcollision = (IPhysicsCollision*)FindSystem( VPHYSICS_COLLISION_INTERFACE_VERSION ); + if ( !physcollision ) + return false; + } + return bRet; +} + +bool CBenchmarkApp::SetupSearchPaths() +{ + CFSSteamSetupInfo steamInfo; + steamInfo.m_pDirectoryName = NULL; + steamInfo.m_bOnlyUseDirectoryName = false; + steamInfo.m_bToolsMode = true; + steamInfo.m_bSetSteamDLLPath = true; + steamInfo.m_bSteam = g_pFullFileSystem->IsSteam(); + + if ( FileSystem_SetupSteamEnvironment( steamInfo ) != FS_OK ) + return false; + + CFSMountContentInfo fsInfo; + fsInfo.m_pFileSystem = g_pFullFileSystem; + fsInfo.m_bToolsMode = true; + fsInfo.m_pDirectoryName = steamInfo.m_GameInfoPath; + + if ( FileSystem_MountContent( fsInfo ) != FS_OK ) + return false; + + // Finally, load the search paths for the "GAME" path. + CFSSearchPathsInit searchPathsInit; + searchPathsInit.m_pDirectoryName = steamInfo.m_GameInfoPath; + searchPathsInit.m_pFileSystem = g_pFullFileSystem; + if ( FileSystem_LoadSearchPaths( searchPathsInit ) != FS_OK ) + return false; + + g_pFullFileSystem->AddSearchPath( steamInfo.m_GameInfoPath, "SKIN", PATH_ADD_TO_HEAD ); + + FileSystem_AddSearchPath_Platform( g_pFullFileSystem, steamInfo.m_GameInfoPath ); + + return true; +} + +bool CBenchmarkApp::PreInit( ) +{ + CreateInterfaceFn factory = GetFactory(); + ConnectTier1Libraries( &factory, 1 ); + ConnectTier2Libraries( &factory, 1 ); + ConnectTier3Libraries( &factory, 1 ); + + if ( !g_pFullFileSystem || !physcollision ) + { + Warning( "benchmark is missing a required interface!\n" ); + return false; + } + + return SetupSearchPaths();//( NULL, false, true ); +} + + +void CBenchmarkApp::PostShutdown() +{ + DisconnectTier3Libraries(); + DisconnectTier2Libraries(); + DisconnectTier1Libraries(); +} + +struct baseline_t +{ + float total; + float ray; + float box; +}; + +// current baseline measured on Core2DuoE6600 +baseline_t g_Baselines[] = +{ + { 40.56f, 10.64f, 29.92f}, // bench01a.phy + { 38.13f, 10.76f, 27.37f }, // bicycle01a.phy + { 25.46f, 8.34f, 17.13f }, // furnituretable001a.phy + { 12.65f, 6.02f, 6.62f }, // gravestone003a.phy + { 40.58f, 16.49f, 24.10f }, // combineinnerwall001a.phy +}; + +const float g_TotalBaseline = 157.38f; + +/* +Benchmark models\props_c17\bench01a.phy! + +33.90 ms [1.20 X] 2500/2500 hits +8.95 ms rays [1.19 X] 24.95 ms boxes [1.20 X] +Benchmark models\props_junk\bicycle01a.phy! + +30.55 ms [1.25 X] 803/2500 hits +8.96 ms rays [1.20 X] 21.59 ms boxes [1.27 X] +Benchmark models\props_c17\furnituretable001a.phy! + +20.61 ms [1.24 X] 755/2500 hits +6.88 ms rays [1.21 X] 13.73 ms boxes [1.25 X] +Benchmark models\props_c17\gravestone003a.phy! + +9.13 ms [1.39 X] 2500/2500 hits +4.34 ms rays [1.39 X] 4.79 ms boxes [1.38 X] +Benchmark models\props_combine\combineinnerwall001a.phy! + +33.04 ms [1.23 X] 985/2500 hits +13.37 ms rays [1.23 X] 19.67 ms boxes [1.23 X] + +127.22s total [1.24 X]! +*/ + +#define IMPROVEMENT_FACTOR(x,baseline) (baseline/(x)) +#define IMPROVEMENT_PERCENT(x,baseline) (((baseline-(x)) / baseline) * 100.0f) +#include <windows.h> +//----------------------------------------------------------------------------- +// The application object +//----------------------------------------------------------------------------- +int CBenchmarkApp::Main() +{ + const char *pFileNames[] = + { + "models\\props_c17\\bench01a.phy", + "models\\props_junk\\bicycle01a.phy", + "models\\props_c17\\furnituretable001a.phy", + "models\\props_c17\\gravestone003a.phy", + "models\\props_combine\\combineinnerwall001a.phy", + }; + vcollide_t testModels[ARRAYSIZE(pFileNames)]; + for ( int i = 0; i < ARRAYSIZE(pFileNames); i++ ) + { + ReadPHYFile( pFileNames[i], testModels[i] ); + } + SetPriorityClass( GetCurrentProcess(), REALTIME_PRIORITY_CLASS ); + SetThreadPriority( GetCurrentThread(), THREAD_PRIORITY_HIGHEST ); + float totalTime = 0.0f; + int loopCount = ARRAYSIZE(pFileNames); +#if VPROF_LEVEL > 0 +// loopCount = 3; +#endif + for ( int i = 0; i < loopCount; i++ ) + { + if ( testModels[i].solidCount < 1 ) + { + Msg("Failed to load %s, skipping test!\n", pFileNames[i] ); + continue; + } + Msg("Benchmark %s!\n\n", pFileNames[i] ); + + benchresults_t results; + memset( &results, 0, sizeof(results)); + int numRepeats = 3; +#if VPROF_LEVEL > 0 + numRepeats = 1; +#endif + for ( int j = 0; j < numRepeats; j++ ) + { + Benchmark_PHY( testModels[i].solids[0], &results ); + } + Msg("%.2f ms [%.2f X] %d/%d hits\n", results.totalTime, IMPROVEMENT_FACTOR(results.totalTime, g_Baselines[i].total), results.collisionHits, results.collisionTests); + Msg("%.2f ms rays \t[%.2f X] \t%.2f ms boxes [%.2f X]\n", + results.rayTime, IMPROVEMENT_FACTOR(results.rayTime, g_Baselines[i].ray), + results.boxTime, IMPROVEMENT_FACTOR(results.boxTime, g_Baselines[i].box)); + totalTime += results.totalTime; + } + SetPriorityClass( GetCurrentProcess(), NORMAL_PRIORITY_CLASS ); + + Msg("\n%.2fs total \t[%.2f X]!\n", totalTime, IMPROVEMENT_FACTOR(totalTime, g_TotalBaseline) ); + return 0; +} + + + diff --git a/vphysics/traceperf/traceperf.vpc b/vphysics/traceperf/traceperf.vpc new file mode 100644 index 0000000..6fad00c --- /dev/null +++ b/vphysics/traceperf/traceperf.vpc @@ -0,0 +1,43 @@ +//----------------------------------------------------------------------------- +// VBSP.VPC +// +// Project Script +//----------------------------------------------------------------------------- + +$Macro SRCDIR "..\.." +$Macro OUTBINDIR "$SRCDIR\..\game\bin" + +$Include "$SRCDIR\vpc_scripts\source_exe_con_base.vpc" + +$Configuration +{ + $Compiler + { + $AdditionalIncludeDirectories "$BASE,..\common,..\vmpi" + $PreprocessorDefinitions "$BASE;MACRO_MATHLIB;PROTECTED_THINGS_DISABLE" + $FloatingPointModel "Precise (/fp:precise)" + } +} + +$Project "traceperf" +{ + $Folder "Source Files" + { + $File "stdafx.cpp" + $File "traceperf.cpp" + } + + $Folder "Header Files" + { + $File "stdafx.h" + $File "$SRCDIR\public\vphysics_interface.h" + } + + $Folder "Link Libraries" + { + $Lib appframework + $Lib mathlib + $Lib tier2 + $Lib tier3 + } +} diff --git a/vphysics/vcollide_parse.cpp b/vphysics/vcollide_parse.cpp new file mode 100644 index 0000000..aa6f772 --- /dev/null +++ b/vphysics/vcollide_parse.cpp @@ -0,0 +1,940 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +// +//=============================================================================// +#include "cbase.h" + +#include "vcollide_parse_private.h" + +#include "tier1/strtools.h" +#include "vphysics/constraints.h" +#include "vphysics/vehicles.h" +#include "filesystem_helpers.h" +#include "bspfile.h" +#include "utlbuffer.h" +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +static void ReadVector( const char *pString, Vector& out ) +{ + float x, y, z; + sscanf( pString, "%f %f %f", &x, &y, &z ); + out[0] = x; + out[1] = y; + out[2] = z; +} + +static void ReadAngles( const char *pString, QAngle& out ) +{ + float x, y, z; + sscanf( pString, "%f %f %f", &x, &y, &z ); + out[0] = x; + out[1] = y; + out[2] = z; +} + +static void ReadVector4D( const char *pString, Vector4D& out ) +{ + float x, y, z, w; + sscanf( pString, "%f %f %f %f", &x, &y, &z, &w ); + out[0] = x; + out[1] = y; + out[2] = z; + out[3] = w; +} + +class CVPhysicsParse : public IVPhysicsKeyParser +{ +public: + ~CVPhysicsParse() {} + + CVPhysicsParse( const char *pKeyData ); + void NextBlock( void ); + + const char *GetCurrentBlockName( void ); + bool Finished( void ); + void ParseSolid( solid_t *pSolid, IVPhysicsKeyHandler *unknownKeyHandler ); + void ParseFluid( fluid_t *pFluid, IVPhysicsKeyHandler *unknownKeyHandler ); + void ParseRagdollConstraint( constraint_ragdollparams_t *pConstraint, IVPhysicsKeyHandler *unknownKeyHandler ); + void ParseSurfaceTable( int *table, IVPhysicsKeyHandler *unknownKeyHandler ); + void ParseSurfaceTablePacked( CUtlVector<char> &out ); + void ParseVehicle( vehicleparams_t *pVehicle, IVPhysicsKeyHandler *unknownKeyHandler ); + void ParseCustom( void *pCustom, IVPhysicsKeyHandler *unknownKeyHandler ); + void SkipBlock( void ) { ParseCustom(NULL, NULL); } + +private: + void ParseVehicleAxle( vehicle_axleparams_t &axle ); + void ParseVehicleWheel( vehicle_wheelparams_t &wheel ); + void ParseVehicleSuspension( vehicle_suspensionparams_t &suspension ); + void ParseVehicleBody( vehicle_bodyparams_t &body ); + void ParseVehicleEngine( vehicle_engineparams_t &engine ); + void ParseVehicleEngineBoost( vehicle_engineparams_t &engine ); + void ParseVehicleSteering( vehicle_steeringparams_t &steering ); + + const char *m_pText; + char m_blockName[MAX_KEYVALUE]; +}; + + +CVPhysicsParse::CVPhysicsParse( const char *pKeyData ) +{ + m_pText = pKeyData; + NextBlock(); +} + +void CVPhysicsParse::NextBlock( void ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( !Q_strcmp(value, "{") ) + { + V_strcpy_safe( m_blockName, key ); + return; + } + } + + // Make sure m_blockName is initialized -- should this be done? + m_blockName[ 0 ] = 0; +} + + +const char *CVPhysicsParse::GetCurrentBlockName( void ) +{ + if ( m_pText ) + return m_blockName; + + return NULL; +} + + +bool CVPhysicsParse::Finished( void ) +{ + if ( m_pText ) + return false; + + return true; +} + + +void CVPhysicsParse::ParseSolid( solid_t *pSolid, IVPhysicsKeyHandler *unknownKeyHandler ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + key[0] = 0; + + if ( unknownKeyHandler ) + { + unknownKeyHandler->SetDefaults( pSolid ); + } + else + { + memset( pSolid, 0, sizeof(*pSolid) ); + } + + // disable these until the ragdoll is created + pSolid->params.enableCollisions = false; + + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + { + NextBlock(); + return; + } + + if ( !Q_stricmp( key, "index" ) ) + { + pSolid->index = atoi(value); + } + else if ( !Q_stricmp( key, "name" ) ) + { + Q_strncpy( pSolid->name, value, sizeof(pSolid->name) ); + } + else if ( !Q_stricmp( key, "parent" ) ) + { + Q_strncpy( pSolid->parent, value, sizeof(pSolid->parent) ); + } + else if ( !Q_stricmp( key, "surfaceprop" ) ) + { + Q_strncpy( pSolid->surfaceprop, value, sizeof(pSolid->surfaceprop) ); + } + else if ( !Q_stricmp( key, "mass" ) ) + { + pSolid->params.mass = atof(value); + } + else if ( !Q_stricmp( key, "massCenterOverride" ) ) + { + ReadVector( value, pSolid->massCenterOverride ); + pSolid->params.massCenterOverride = &pSolid->massCenterOverride; + } + else if ( !Q_stricmp( key, "inertia" ) ) + { + pSolid->params.inertia = atof(value); + } + else if ( !Q_stricmp( key, "damping" ) ) + { + pSolid->params.damping = atof(value); + } + else if ( !Q_stricmp( key, "rotdamping" ) ) + { + pSolid->params.rotdamping = atof(value); + } + else if ( !Q_stricmp( key, "volume" ) ) + { + pSolid->params.volume = atof(value); + } + else if ( !Q_stricmp( key, "drag" ) ) + { + pSolid->params.dragCoefficient = atof(value); + } + else if ( !Q_stricmp( key, "rollingdrag" ) ) + { + //pSolid->params.rollingDrag = atof(value); + } + else + { + if ( unknownKeyHandler ) + { + unknownKeyHandler->ParseKeyValue( pSolid, key, value ); + } + } + } +} + +void CVPhysicsParse::ParseRagdollConstraint( constraint_ragdollparams_t *pConstraint, IVPhysicsKeyHandler *unknownKeyHandler ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + if ( unknownKeyHandler ) + { + unknownKeyHandler->SetDefaults( pConstraint ); + } + else + { + memset( pConstraint, 0, sizeof(*pConstraint) ); + pConstraint->childIndex = -1; + pConstraint->parentIndex = -1; + } + + // BUGBUG: xmin/xmax, ymin/ymax, zmin/zmax specify clockwise rotations. + // BUGBUG: HL rotations are counter-clockwise, so reverse these limits at some point!!! + pConstraint->useClockwiseRotations = true; + + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + { + NextBlock(); + return; + } + + if ( !Q_stricmp( key, "parent" ) ) + { + pConstraint->parentIndex = atoi( value ); + } + else if ( !Q_stricmp( key, "child" ) ) + { + pConstraint->childIndex = atoi( value ); + } + else if ( !Q_stricmp( key, "xmin" ) ) + { + pConstraint->axes[0].minRotation = atof( value ); + } + else if ( !Q_stricmp( key, "xmax" ) ) + { + pConstraint->axes[0].maxRotation = atof( value ); + } + else if ( !Q_stricmp( key, "xfriction" ) ) + { + pConstraint->axes[0].angularVelocity = 0; + pConstraint->axes[0].torque = atof( value ); + } + else if ( !Q_stricmp( key, "ymin" ) ) + { + pConstraint->axes[1].minRotation = atof( value ); + } + else if ( !Q_stricmp( key, "ymax" ) ) + { + pConstraint->axes[1].maxRotation = atof( value ); + } + else if ( !Q_stricmp( key, "yfriction" ) ) + { + pConstraint->axes[1].angularVelocity = 0; + pConstraint->axes[1].torque = atof( value ); + } + else if ( !Q_stricmp( key, "zmin" ) ) + { + pConstraint->axes[2].minRotation = atof( value ); + } + else if ( !Q_stricmp( key, "zmax" ) ) + { + pConstraint->axes[2].maxRotation = atof( value ); + } + else if ( !Q_stricmp( key, "zfriction" ) ) + { + pConstraint->axes[2].angularVelocity = 0; + pConstraint->axes[2].torque = atof( value ); + } + else + { + if ( unknownKeyHandler ) + { + unknownKeyHandler->ParseKeyValue( pConstraint, key, value ); + } + } + } +} + + +void CVPhysicsParse::ParseFluid( fluid_t *pFluid, IVPhysicsKeyHandler *unknownKeyHandler ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + pFluid->index = -1; + if ( unknownKeyHandler ) + { + unknownKeyHandler->SetDefaults( pFluid ); + } + else + { + memset( pFluid, 0, sizeof(*pFluid) ); + // HACKHACK: This is a reasonable default even though it is hardcoded + V_strcpy_safe( pFluid->surfaceprop, "water" ); + } + + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + { + NextBlock(); + return; + } + + if ( !Q_stricmp( key, "index" ) ) + { + pFluid->index = atoi( value ); + } + else if ( !Q_stricmp( key, "damping" ) ) + { + pFluid->params.damping = atof( value ); + } + else if ( !Q_stricmp( key, "surfaceplane" ) ) + { + ReadVector4D( value, pFluid->params.surfacePlane ); + } + else if ( !Q_stricmp( key, "currentvelocity" ) ) + { + ReadVector( value, pFluid->params.currentVelocity ); + } + else if ( !Q_stricmp( key, "contents" ) ) + { + pFluid->params.contents = atoi( value ); + } + else if ( !Q_stricmp( key, "surfaceprop" ) ) + { + Q_strncpy( pFluid->surfaceprop, value, sizeof(pFluid->surfaceprop) ); + } + else + { + if ( unknownKeyHandler ) + { + unknownKeyHandler->ParseKeyValue( pFluid, key, value ); + } + } + } +} + +void CVPhysicsParse::ParseSurfaceTable( int *table, IVPhysicsKeyHandler *unknownKeyHandler ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + { + NextBlock(); + return; + } + + int propIndex = physprops->GetSurfaceIndex( key ); + int tableIndex = atoi(value); + if ( tableIndex >= 0 && tableIndex < 128 ) + { + table[tableIndex] = propIndex; + } + } +} + +void CVPhysicsParse::ParseSurfaceTablePacked( CUtlVector<char> &out ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + int lastIndex = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + { + NextBlock(); + return; + } + + int len = Q_strlen( key ); + int outIndex = out.AddMultipleToTail( len + 1 ); + memcpy( &out[outIndex], key, len+1 ); + int tableIndex = atoi(value); + Assert( tableIndex == lastIndex + 1); + lastIndex = tableIndex; + } +} + +void CVPhysicsParse::ParseVehicleAxle( vehicle_axleparams_t &axle ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + memset( &axle, 0, sizeof(axle) ); + key[0] = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + return; + + // parse subchunks + if ( value[0] == '{' ) + { + if ( !Q_stricmp( key, "wheel" ) ) + { + ParseVehicleWheel( axle.wheels ); + } + else if ( !Q_stricmp( key, "suspension" ) ) + { + ParseVehicleSuspension( axle.suspension ); + } + else + { + SkipBlock(); + } + } + else if ( !Q_stricmp( key, "offset" ) ) + { + ReadVector( value, axle.offset ); + } + else if ( !Q_stricmp( key, "wheeloffset" ) ) + { + ReadVector( value, axle.wheelOffset ); + } + else if ( !Q_stricmp( key, "torquefactor" ) ) + { + axle.torqueFactor = atof( value ); + } + else if ( !Q_stricmp( key, "brakefactor" ) ) + { + axle.brakeFactor = atof( value ); + } + } +} + +void CVPhysicsParse::ParseVehicleWheel( vehicle_wheelparams_t &wheel ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + return; + + if ( !Q_stricmp( key, "radius" ) ) + { + wheel.radius = atof( value ); + } + else if ( !Q_stricmp( key, "mass" ) ) + { + wheel.mass = atof( value ); + } + else if ( !Q_stricmp( key, "inertia" ) ) + { + wheel.inertia = atof( value ); + } + else if ( !Q_stricmp( key, "damping" ) ) + { + wheel.damping = atof( value ); + } + else if ( !Q_stricmp( key, "rotdamping" ) ) + { + wheel.rotdamping = atof( value ); + } + else if ( !Q_stricmp( key, "frictionscale" ) ) + { + wheel.frictionScale = atof( value ); + } + else if ( !Q_stricmp( key, "material" ) ) + { + wheel.materialIndex = physprops->GetSurfaceIndex( value ); + } + else if ( !Q_stricmp( key, "skidmaterial" ) ) + { + wheel.skidMaterialIndex = physprops->GetSurfaceIndex( value ); + } + else if ( !Q_stricmp( key, "brakematerial" ) ) + { + wheel.brakeMaterialIndex = physprops->GetSurfaceIndex( value ); + } + } +} + + +void CVPhysicsParse::ParseVehicleSuspension( vehicle_suspensionparams_t &suspension ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + return; + + if ( !Q_stricmp( key, "springconstant" ) ) + { + suspension.springConstant = atof( value ); + } + else if ( !Q_stricmp( key, "springdamping" ) ) + { + suspension.springDamping = atof( value ); + } + else if ( !Q_stricmp( key, "stabilizerconstant" ) ) + { + suspension.stabilizerConstant = atof( value ); + } + else if ( !Q_stricmp( key, "springdampingcompression" ) ) + { + suspension.springDampingCompression = atof( value ); + } + else if ( !Q_stricmp( key, "maxbodyforce" ) ) + { + suspension.maxBodyForce = atof( value ); + } + } +} + + +void CVPhysicsParse::ParseVehicleBody( vehicle_bodyparams_t &body ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + return; + + if ( !Q_stricmp( key, "massCenterOverride" ) ) + { + ReadVector( value, body.massCenterOverride ); + } + else if ( !Q_stricmp( key, "addgravity" ) ) + { + body.addGravity = atof( value ); + } + else if ( !Q_stricmp( key, "maxAngularVelocity" ) ) + { + body.maxAngularVelocity = atof( value ); + } + else if ( !Q_stricmp( key, "massOverride" ) ) + { + body.massOverride = atof( value ); + } + else if ( !Q_stricmp( key, "tiltforce" ) ) + { + body.tiltForce = atof( value ); + } + else if ( !Q_stricmp( key, "tiltforceheight" ) ) + { + body.tiltForceHeight = atof( value ); + } + else if ( !Q_stricmp( key, "countertorquefactor" ) ) + { + body.counterTorqueFactor = atof( value ); + } + else if ( !Q_stricmp( key, "keepuprighttorque" ) ) + { + body.keepUprightTorque = atof( value ); + } + } +} + + +void CVPhysicsParse::ParseVehicleEngineBoost( vehicle_engineparams_t &engine ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + return; + // parse subchunks + if ( !Q_stricmp( key, "force" ) ) + { + engine.boostForce = atof( value ); + } + else if ( !Q_stricmp( key, "duration" ) ) + { + engine.boostDuration = atof( value ); + } + else if ( !Q_stricmp( key, "delay" ) ) + { + engine.boostDelay = atof( value ); + } + else if ( !Q_stricmp( key, "maxspeed" ) ) + { + engine.boostMaxSpeed = atof( value ); + } + else if ( !Q_stricmp( key, "torqueboost" ) ) + { + engine.torqueBoost = atoi( value ) != 0 ? true : false; + } + + } +} + +void CVPhysicsParse::ParseVehicleEngine( vehicle_engineparams_t &engine ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + return; + // parse subchunks + if ( value[0] == '{' ) + { + if ( !Q_stricmp( key, "boost" ) ) + { + ParseVehicleEngineBoost( engine ); + } + else + { + SkipBlock(); + } + } + else if ( !Q_stricmp( key, "gear" ) ) + { + // Protect against exploits/overruns + if ( engine.gearCount < ARRAYSIZE(engine.gearRatio) ) + { + engine.gearRatio[engine.gearCount] = atof( value ); + engine.gearCount++; + } + else + { + Assert( 0 ); + } + } + else if ( !Q_stricmp( key, "horsepower" ) ) + { + engine.horsepower = atof( value ); + } + else if ( !Q_stricmp( key, "maxSpeed" ) ) + { + engine.maxSpeed = atof( value ); + } + else if ( !Q_stricmp( key, "maxReverseSpeed" ) ) + { + engine.maxRevSpeed = atof( value ); + } + else if ( !Q_stricmp( key, "axleratio" ) ) + { + engine.axleRatio = atof( value ); + } + else if ( !Q_stricmp( key, "maxRPM" ) ) + { + engine.maxRPM = atof( value ); + } + else if ( !Q_stricmp( key, "throttleTime" ) ) + { + engine.throttleTime = atof( value ); + } + else if ( !Q_stricmp( key, "AutoTransmission" ) ) + { + engine.isAutoTransmission = atoi( value ) != 0 ? true : false; + } + else if ( !Q_stricmp( key, "shiftUpRPM" ) ) + { + engine.shiftUpRPM = atof( value ); + } + else if ( !Q_stricmp( key, "shiftDownRPM" ) ) + { + engine.shiftDownRPM = atof( value ); + } + else if ( !Q_stricmp( key, "autobrakeSpeedGain" ) ) + { + engine.autobrakeSpeedGain = atof( value ); + } + else if ( !Q_stricmp( key, "autobrakeSpeedFactor" ) ) + { + engine.autobrakeSpeedFactor = atof( value ); + } + } +} + + +void CVPhysicsParse::ParseVehicleSteering( vehicle_steeringparams_t &steering ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + return; + // parse subchunks + if ( !Q_stricmp( key, "degreesSlow" ) ) + { + steering.degreesSlow = atof( value ); + } + else if ( !Q_stricmp( key, "degreesFast" ) ) + { + steering.degreesFast = atof( value ); + } + else if ( !Q_stricmp( key, "degreesBoost" ) ) + { + steering.degreesBoost = atof( value ); + } + else if ( !Q_stricmp( key, "fastcarspeed" ) ) + { + steering.speedFast = atof( value ); + } + else if ( !Q_stricmp( key, "slowcarspeed" ) ) + { + steering.speedSlow = atof( value ); + } + else if ( !Q_stricmp( key, "slowsteeringrate" ) ) + { + steering.steeringRateSlow = atof( value ); + } + else if ( !Q_stricmp( key, "faststeeringrate" ) ) + { + steering.steeringRateFast = atof( value ); + } + else if ( !Q_stricmp( key, "steeringRestRateSlow" ) ) + { + steering.steeringRestRateSlow = atof( value ); + } + else if ( !Q_stricmp( key, "steeringRestRateFast" ) ) + { + steering.steeringRestRateFast = atof( value ); + } + else if ( !Q_stricmp( key, "throttleSteeringRestRateFactor" ) ) + { + steering.throttleSteeringRestRateFactor = atof( value ); + } + else if ( !Q_stricmp( key, "boostSteeringRestRateFactor" ) ) + { + steering.boostSteeringRestRateFactor = atof( value ); + } + else if ( !Q_stricmp( key, "boostSteeringRateFactor" ) ) + { + steering.boostSteeringRateFactor = atof( value ); + } + else if ( !Q_stricmp( key, "steeringExponent" ) ) + { + steering.steeringExponent = atof( value ); + } + else if ( !Q_stricmp( key, "turnThrottleReduceSlow" ) ) + { + steering.turnThrottleReduceSlow = atof( value ); + } + else if ( !Q_stricmp( key, "turnThrottleReduceFast" ) ) + { + steering.turnThrottleReduceFast = atof( value ); + } + else if ( !Q_stricmp( key, "brakeSteeringRateFactor" ) ) + { + steering.brakeSteeringRateFactor = atof( value ); + } + else if ( !Q_stricmp( key, "powerSlideAccel" ) ) + { + steering.powerSlideAccel = atof( value ); + } + else if ( !Q_stricmp( key, "skidallowed" ) ) + { + steering.isSkidAllowed = atoi( value ) != 0 ? true : false; + } + else if ( !Q_stricmp( key, "dustcloud" ) ) + { + steering.dustCloud = atoi( value ) != 0 ? true : false; + } + } +} + +void CVPhysicsParse::ParseVehicle( vehicleparams_t *pVehicle, IVPhysicsKeyHandler *unknownKeyHandler ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + if ( unknownKeyHandler ) + { + unknownKeyHandler->SetDefaults( pVehicle ); + } + else + { + memset( pVehicle, 0, sizeof(*pVehicle) ); + } + + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + if ( key[0] == '}' ) + { + NextBlock(); + return; + } + + // parse subchunks + if ( value[0] == '{' ) + { + if ( !Q_stricmp( key, "axle" ) ) + { + // Protect against exploits/overruns + if ( pVehicle->axleCount < ARRAYSIZE(pVehicle->axles) ) + { + ParseVehicleAxle( pVehicle->axles[pVehicle->axleCount] ); + pVehicle->axleCount++; + } + else + { + Assert( 0 ); + } + } + else if ( !Q_stricmp( key, "body" ) ) + { + ParseVehicleBody( pVehicle->body ); + } + else if ( !Q_stricmp( key, "engine" ) ) + { + ParseVehicleEngine( pVehicle->engine ); + } + else if ( !Q_stricmp( key, "steering" ) ) + { + ParseVehicleSteering( pVehicle->steering ); + } + else + { + SkipBlock(); + } + } + else if ( !Q_stricmp( key, "wheelsperaxle" ) ) + { + pVehicle->wheelsPerAxle = atoi( value ); + } + } +} + +void CVPhysicsParse::ParseCustom( void *pCustom, IVPhysicsKeyHandler *unknownKeyHandler ) +{ + char key[MAX_KEYVALUE], value[MAX_KEYVALUE]; + + key[0] = 0; + int indent = 0; + if ( unknownKeyHandler ) + { + unknownKeyHandler->SetDefaults( pCustom ); + } + + while ( m_pText ) + { + m_pText = ParseKeyvalue( m_pText, key, value ); + + if ( m_pText ) + { + if ( key[0] == '{' ) + { + indent++; + } + else if ( value[0] == '{' ) + { + // They've got a named block here + // Increase our indent, and let them parse the key + indent++; + if ( unknownKeyHandler ) + { + unknownKeyHandler->ParseKeyValue( pCustom, key, value ); + } + } + else if ( key[0] == '}' ) + { + indent--; + if ( indent < 0 ) + { + NextBlock(); + return; + } + } + else + { + if ( unknownKeyHandler ) + { + unknownKeyHandler->ParseKeyValue( pCustom, key, value ); + } + } + } + } +} + +IVPhysicsKeyParser *CreateVPhysicsKeyParser( const char *pKeyData ) +{ + return new CVPhysicsParse( pKeyData ); +} + +void DestroyVPhysicsKeyParser( IVPhysicsKeyParser *pParser ) +{ + delete pParser; +} + +//----------------------------------------------------------------------------- +// Helper functions for parsing script file +//----------------------------------------------------------------------------- + +const char *ParseKeyvalue( const char *pBuffer, char (&key)[MAX_KEYVALUE], char (&value)[MAX_KEYVALUE] ) +{ + // Make sure value is always null-terminated. + value[0] = 0; + + pBuffer = ParseFile( pBuffer, key, NULL ); + + // no value on a close brace + if ( key[0] == '}' && key[1] == 0 ) + { + value[0] = 0; + return pBuffer; + } + + Q_strlower( key ); + + pBuffer = ParseFile( pBuffer, value, NULL ); + + Q_strlower( value ); + + return pBuffer; +} diff --git a/vphysics/vcollide_parse_private.h b/vphysics/vcollide_parse_private.h new file mode 100644 index 0000000..3c32eb9 --- /dev/null +++ b/vphysics/vcollide_parse_private.h @@ -0,0 +1,28 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef VCOLLIDE_PARSE_PRIVATE_H +#define VCOLLIDE_PARSE_PRIVATE_H +#ifdef _WIN32 +#pragma once +#endif + +#include "vcollide_parse.h" + +#define MAX_KEYVALUE 1024 + +class IVPhysicsKeyParser; +class CPackedPhysicsDescription; + +const char *ParseKeyvalue( const char *pBuffer, OUT_Z_ARRAY char (&key)[MAX_KEYVALUE], OUT_Z_ARRAY char (&value)[MAX_KEYVALUE] ); +IVPhysicsKeyParser *CreateVPhysicsKeyParser( const char *pKeyData ); +void DestroyVPhysicsKeyParser( IVPhysicsKeyParser * ); +const char *PackVCollideText( IPhysicsCollision *physcollision, const char *pTextIn, int *pSizeOut, bool storeSolidNames, bool storeSurfacepropsAsNames ); +CPackedPhysicsDescription *CreatePackedDescription( const char *pPackedBuffer, int packedSize ); +void DestroyPackedDescription( CPackedPhysicsDescription *pPhysics ); + +#endif // VCOLLIDE_PARSE_PRIVATE_H diff --git a/vphysics/vphysics.vpc b/vphysics/vphysics.vpc new file mode 100644 index 0000000..9b4ee3f --- /dev/null +++ b/vphysics/vphysics.vpc @@ -0,0 +1,136 @@ +//----------------------------------------------------------------------------- +// VPHYSICS.VPC +// +// Project Script +//----------------------------------------------------------------------------- + +$macro SRCDIR ".." +$Macro OUTBINDIR "$SRCDIR\..\game\bin" + +$include "$SRCDIR\vpc_scripts\source_dll_base.vpc" + +$Configuration +{ + $Compiler + { + $AdditionalIncludeDirectories "$BASE;$SRCDIR\ivp\ivp_intern;$SRCDIR\ivp\ivp_collision;$SRCDIR\ivp\ivp_physics;$SRCDIR\ivp\ivp_surface_manager;$SRCDIR\ivp\ivp_utility;$SRCDIR\ivp\ivp_controller;$SRCDIR\ivp\ivp_compact_builder;$SRCDIR\ivp\havana\havok;$SRCDIR\ivp\havana" + $PreprocessorDefinitions "$BASE;VPHYSICS_EXPORTS;HAVANA_CONSTRAINTS;HAVOK_MOPP" + $Create/UsePrecompiledHeader "Use Precompiled Header (/Yu)" + $Create/UsePCHThroughFile "cbase.h" + $PrecompiledHeaderFile "$(IntDir)\vphysics.pch" + // Language + $EnableRunTimeTypeInfo "No (/GR-)" + } + $Compiler [$WIN32] + { + $EnableEnhancedInstructionSet "Streaming SIMD Extensions (/arch:SSE)" + } + + $Linker + { + $AdditionalDependencies "$BASE odbc32.lib odbccp32.lib" [$WIN32] + $SystemLibraries "iconv" [$OSXALL] + } +} + +$Project "vphysics" +{ + $Folder "Source Files" + { + $File "stdafx.cpp" + { + $Configuration + { + $Compiler + { + $Create/UsePrecompiledHeader "Create Precompiled Header (/Yc)" + } + } + } + + $File "convert.cpp" \ + "$SRCDIR\public\filesystem_helpers.cpp" + { + $Configuration + { + $Compiler + { + $Create/UsePrecompiledHeader "Not Using Precompiled Headers" + } + } + } + + $File "ledgewriter.cpp" + $File "main.cpp" + $File "physics_airboat.cpp" + $File "physics_collide.cpp" + $File "physics_constraint.cpp" + $File "physics_controller_raycast_vehicle.cpp" + $File "physics_environment.cpp" + $File "physics_fluid.cpp" + $File "physics_friction.cpp" + $File "physics_material.cpp" + $File "physics_motioncontroller.cpp" + $File "physics_object.cpp" + $File "physics_shadow.cpp" + $File "physics_spring.cpp" + $File "physics_vehicle.cpp" + $File "physics_virtualmesh.cpp" + $File "trace.cpp" + $File "vcollide_parse.cpp" + $File "vphysics_saverestore.cpp" + } + + $Folder "Header Files" + { + $File "cbase.h" + $File "convert.h" + $File "linear_solver.h" + $File "physics_airboat.h" + $File "physics_constraint.h" + $File "physics_controller_raycast_vehicle.h" + $File "physics_environment.h" + $File "physics_fluid.h" + $File "physics_friction.h" + $File "physics_material.h" + $File "physics_motioncontroller.h" + $File "physics_object.h" + $File "physics_shadow.h" + $File "physics_spring.h" + $File "physics_trace.h" + $File "physics_vehicle.h" + $File "vcollide_parse_private.h" + $File "vphysics_internal.h" + $File "vphysics_saverestore.h" + } + + $Folder "Public Header Files" + { + $File "$SRCDIR\public\vphysics\collision_set.h" + $File "$SRCDIR\public\vphysics\constraints.h" + $File "$SRCDIR\public\datamap.h" + $File "$SRCDIR\public\filesystem_helpers.h" + $File "$SRCDIR\public\vphysics\friction.h" + $File "$SRCDIR\public\vphysics\object_hash.h" + $File "$SRCDIR\public\vphysics\performance.h" + $File "$SRCDIR\public\vphysics\player_controller.h" + $File "$SRCDIR\public\vphysics\stats.h" + $File "$SRCDIR\public\vcollide.h" + $File "$SRCDIR\public\vcollide_parse.h" + $File "$SRCDIR\public\vphysics\vehicles.h" + $File "$SRCDIR\public\vphysics_interface.h" + $File "$SRCDIR\public\vphysics_interfaceV30.h" + } + + $Folder "Link Libraries" + { + $Lib "$LIBCOMMON/havana_constraints" + $Lib "$LIBCOMMON/hk_base" + $Lib "$LIBCOMMON/hk_math" + $Lib "$LIBCOMMON/ivp_compactbuilder" + $Lib "$LIBCOMMON/ivp_physics" + $Lib mathlib + $Lib tier2 + } + +} diff --git a/vphysics/vphysics_internal.h b/vphysics/vphysics_internal.h new file mode 100644 index 0000000..0114882 --- /dev/null +++ b/vphysics/vphysics_internal.h @@ -0,0 +1,30 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef VPHYSICS_INTERNAL_H +#define VPHYSICS_INTERNAL_H +#ifdef _WIN32 +#pragma once +#endif + +#include "tier0/memalloc.h" + +extern class IPhysics *g_PhysicsInternal; + +//----------------------------------------------------------------------------- +// Memory debugging +//----------------------------------------------------------------------------- +#if defined(_DEBUG) || defined(USE_MEM_DEBUG) +#define BEGIN_IVP_ALLOCATION() MemAlloc_PushAllocDbgInfo("IVP: " __FILE__ , __LINE__ ) +#define END_IVP_ALLOCATION() MemAlloc_PopAllocDbgInfo() +#else +#define BEGIN_IVP_ALLOCATION() 0 +#define END_IVP_ALLOCATION() 0 +#endif + + +#endif // VPHYSICS_INTERNAL_H diff --git a/vphysics/vphysics_saverestore.cpp b/vphysics/vphysics_saverestore.cpp new file mode 100644 index 0000000..4908f80 --- /dev/null +++ b/vphysics/vphysics_saverestore.cpp @@ -0,0 +1,222 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +//=============================================================================// + +#include "cbase.h" +// memdbgon must be the last include file in a .cpp file!!! +#include "tier0/memdbgon.h" + +//----------------------------------------------------------------------------- +// Phys pointer association +//----------------------------------------------------------------------------- +static CUtlMap<void *, void *> s_VPhysPtrMap( 0, 0, DefLessFunc(void *) ); + + +CVPhysPtrSaveRestoreOps g_VPhysPtrSaveRestoreOps; +CVPhysPtrUtlVectorSaveRestoreOps g_VPhysPtrUtlVectorSaveRestoreOps; + + +//----------------------------------------------------------------------------- +// Phys pointer association +//----------------------------------------------------------------------------- +static void AddPtrAssociation( void *pOldValue, void *pNewValue ) +{ + s_VPhysPtrMap.Insert( pOldValue, pNewValue ); +} + + +//----------------------------------------------------------------------------- +// Purpose: Save/load part of CPhysicsEnvironment +//----------------------------------------------------------------------------- +static bool NoPhysSaveFunc( const physsaveparams_t ¶ms, void * ) +{ + AssertMsg( 0, "Physics cannot save the specified type" ); + return false; +} + +bool CPhysicsEnvironment::Save( const physsaveparams_t ¶ms ) +{ + const PhysInterfaceId_t type = params.type; + Assert( type >= 0 && type < PIID_NUM_TYPES ); + + static PhysSaveFunc_t saveFuncs[PIID_NUM_TYPES] = + { + NoPhysSaveFunc, + (PhysSaveFunc_t)SavePhysicsObject, + (PhysSaveFunc_t)SavePhysicsFluidController, + (PhysSaveFunc_t)SavePhysicsSpring, + (PhysSaveFunc_t)SavePhysicsConstraintGroup, + (PhysSaveFunc_t)SavePhysicsConstraint, + (PhysSaveFunc_t)SavePhysicsShadowController, + (PhysSaveFunc_t)SavePhysicsPlayerController, + (PhysSaveFunc_t)SavePhysicsMotionController, + (PhysSaveFunc_t)SavePhysicsVehicleController, + }; + + if ( type >= 0 && type < PIID_NUM_TYPES ) + { + params.pSave->WriteInt( (int *)¶ms.pObject ); + return (*saveFuncs[type])( params, params.pObject ); + } + return false; +} + +static bool NoPhysRestoreFunc( const physrestoreparams_t ¶ms, void ** ) +{ + AssertMsg( 0, "Physics cannot save the specified type" ); + return false; +} + +CVPhysPtrSaveRestoreOps::CVPhysPtrSaveRestoreOps() +{ +} + + +void CPhysicsEnvironment::PreRestore( const physprerestoreparams_t ¶ms ) +{ + g_VPhysPtrSaveRestoreOps.PreRestore(); + for ( int i = 0; i < params.recreatedObjectCount; i++ ) + { + AddPtrAssociation( params.recreatedObjectList[i].pOldObject, params.recreatedObjectList[i].pNewObject ); + } +} + +bool CPhysicsEnvironment::Restore( const physrestoreparams_t ¶ms ) +{ + const PhysInterfaceId_t type = params.type; + Assert( type >= 0 && type < PIID_NUM_TYPES ); + + static PhysRestoreFunc_t restoreFuncs[PIID_NUM_TYPES] = + { + NoPhysRestoreFunc, + (PhysRestoreFunc_t)RestorePhysicsObject, + (PhysRestoreFunc_t)RestorePhysicsFluidController, + (PhysRestoreFunc_t)RestorePhysicsSpring, + (PhysRestoreFunc_t)RestorePhysicsConstraintGroup, + (PhysRestoreFunc_t)RestorePhysicsConstraint, + (PhysRestoreFunc_t)RestorePhysicsShadowController, + (PhysRestoreFunc_t)RestorePhysicsPlayerController, + (PhysRestoreFunc_t)RestorePhysicsMotionController, + (PhysRestoreFunc_t)RestorePhysicsVehicleController, + }; + + if ( type >= 0 && type < PIID_NUM_TYPES ) + { + void *pOldObject; + params.pRestore->ReadInt( (int *)&pOldObject ); + if ( (*restoreFuncs[type])( params, params.ppObject ) ) + { + AddPtrAssociation( pOldObject, *params.ppObject ); + if ( type == PIID_IPHYSICSOBJECT ) + { + m_objects.AddToTail( (IPhysicsObject *)(*params.ppObject) ); + } + return true; + } + } + return false; +} + +void CPhysicsEnvironment::PostRestore() +{ + g_VPhysPtrSaveRestoreOps.PostRestore(); +} + + +//----------------------------------------------------------------------------- +// Purpose: Fixes up pointers beteween vphysics objects +//----------------------------------------------------------------------------- + +void CVPhysPtrSaveRestoreOps::Save( const SaveRestoreFieldInfo_t &fieldInfo, ISave *pSave ) +{ + int *pField = (int *)fieldInfo.pField; + int nObjects = fieldInfo.pTypeDesc->fieldSize; + for ( int i = 0; i < nObjects; i++ ) + { + pSave->WriteInt( pField ); + ++pField; + } +} + +//------------------------------------- + +void CVPhysPtrSaveRestoreOps::PreRestore() +{ + Assert( s_VPhysPtrMap.Count() == 0 ); +} + +//------------------------------------- + +void CVPhysPtrSaveRestoreOps::Restore( const SaveRestoreFieldInfo_t &fieldInfo, IRestore *pRestore ) +{ + void **ppField = (void **)fieldInfo.pField; + int nObjects = fieldInfo.pTypeDesc->fieldSize; + for ( int i = 0; i < nObjects; i++ ) + { + pRestore->ReadInt( (int *)ppField ); + + int iNewVal = s_VPhysPtrMap.Find( *ppField ); + if ( iNewVal != s_VPhysPtrMap.InvalidIndex() ) + { + *ppField = s_VPhysPtrMap[iNewVal]; + } + else + { + *ppField = NULL; + } + + ++ppField; + } +} + +//------------------------------------- + +void CVPhysPtrSaveRestoreOps::PostRestore() +{ + s_VPhysPtrMap.RemoveAll(); + PostRestorePhysicsObject(); + PostRestorePhysicsConstraintGroup(); +} + +//----------------------------------------------------------------------------- + +void CVPhysPtrUtlVectorSaveRestoreOps::Save( const SaveRestoreFieldInfo_t &fieldInfo, ISave *pSave ) +{ + Assert( fieldInfo.pTypeDesc->fieldSize == 1 ); + + VPhysPtrVector *pUtlVector = (VPhysPtrVector*)fieldInfo.pField; + int nObjects = pUtlVector->Count(); + pSave->WriteInt( &nObjects ); + for ( int i = 0; i < nObjects; i++ ) + { + pSave->WriteInt( &pUtlVector->Element(i) ); + } +} + +void CVPhysPtrUtlVectorSaveRestoreOps::Restore( const SaveRestoreFieldInfo_t &fieldInfo, IRestore *pRestore ) +{ + Assert( fieldInfo.pTypeDesc->fieldSize == 1 ); + + VPhysPtrVector *pUtlVector = (VPhysPtrVector*)fieldInfo.pField; + + int nObjects; + pRestore->ReadInt( &nObjects ); + pUtlVector->AddMultipleToTail( nObjects ); + for ( int i = 0; i < nObjects; i++ ) + { + void **ppElem = (void**)(&pUtlVector->Element(i)); + pRestore->ReadInt( (int*)ppElem ); + + int iNewVal = s_VPhysPtrMap.Find( *ppElem ); + if ( iNewVal != s_VPhysPtrMap.InvalidIndex() ) + { + *ppElem = s_VPhysPtrMap[iNewVal]; + } + else + { + *ppElem = NULL; + } + } +} diff --git a/vphysics/vphysics_saverestore.h b/vphysics/vphysics_saverestore.h new file mode 100644 index 0000000..c18df61 --- /dev/null +++ b/vphysics/vphysics_saverestore.h @@ -0,0 +1,119 @@ +//========= Copyright Valve Corporation, All rights reserved. ============// +// +// Purpose: +// +// $NoKeywords: $ +//=============================================================================// + +#ifndef VPHYSICS_SAVERESTORE_H +#define VPHYSICS_SAVERESTORE_H + +#if defined( _WIN32 ) +#pragma once +#endif + + +#include "datamap.h" +#include "utlmap.h" +#include "isaverestore.h" +#include "utlvector.h" + + +//------------------------------------- + +class ISave; +class IRestore; + +class CPhysicsObject; +class CPhysicsFluidController; +class CPhysicsSpring; +class CPhysicsConstraint; +class CPhysicsConstraintGroup; +class CShadowController; +class CPlayerController; +class CPhysicsMotionController; +class CVehicleController; +struct physsaveparams_t; +struct physrestoreparams_t; +class ISaveRestoreOps; + +//----------------------------------------------------------------------------- +// Purpose: Fixes up pointers beteween vphysics objects +//----------------------------------------------------------------------------- + +class CVPhysPtrSaveRestoreOps : public CDefSaveRestoreOps +{ +public: + CVPhysPtrSaveRestoreOps(); + void Save( const SaveRestoreFieldInfo_t &fieldInfo, ISave *pSave ); + void PreRestore(); + void PostRestore(); + void Restore( const SaveRestoreFieldInfo_t &fieldInfo, IRestore *pRestore ); +}; + +extern CVPhysPtrSaveRestoreOps g_VPhysPtrSaveRestoreOps; + +#define DEFINE_VPHYSPTR(name) \ + { FIELD_CUSTOM, #name, { offsetof(classNameTypedef,name), 0 }, 1, FTYPEDESC_SAVE, NULL, &g_VPhysPtrSaveRestoreOps, NULL } + +#define DEFINE_VPHYSPTR_ARRAY(name,count) \ + { FIELD_CUSTOM, #name, { offsetof(classNameTypedef,name), 0 }, count, FTYPEDESC_SAVE, NULL, &g_VPhysPtrSaveRestoreOps, NULL } + + +//----------------------------------------------------------------------------- + +class CVPhysPtrUtlVectorSaveRestoreOps : public CVPhysPtrSaveRestoreOps +{ +public: + void Save( const SaveRestoreFieldInfo_t &fieldInfo, ISave *pSave ); + void Restore( const SaveRestoreFieldInfo_t &fieldInfo, IRestore *pRestore ); + +private: + typedef CUtlVector<int> VPhysPtrVector; +}; + +extern CVPhysPtrUtlVectorSaveRestoreOps g_VPhysPtrUtlVectorSaveRestoreOps; + +#define DEFINE_VPHYSPTR_UTLVECTOR(name) \ + { FIELD_CUSTOM, #name, { offsetof(classNameTypedef,name), 0 }, 1, FTYPEDESC_SAVE, NULL, &g_VPhysPtrUtlVectorSaveRestoreOps, NULL } + + +//----------------------------------------------------------------------------- + +typedef bool (*PhysSaveFunc_t)( const physsaveparams_t ¶ms, void *pCastedObject ); // second parameter for convenience +typedef bool (*PhysRestoreFunc_t)( const physrestoreparams_t ¶ms, void **ppCastedObject ); + +bool SavePhysicsObject( const physsaveparams_t ¶ms, CPhysicsObject *pObject ); +bool RestorePhysicsObject( const physrestoreparams_t ¶ms, CPhysicsObject **ppObject ); + +bool SavePhysicsFluidController( const physsaveparams_t ¶ms, CPhysicsFluidController *pFluidObject ); +bool RestorePhysicsFluidController( const physrestoreparams_t ¶ms, CPhysicsFluidController **ppFluidObject ); + +bool SavePhysicsSpring( const physsaveparams_t ¶ms, CPhysicsSpring *pSpring ); +bool RestorePhysicsSpring( const physrestoreparams_t ¶ms, CPhysicsSpring **ppSpring ); + +bool SavePhysicsConstraint( const physsaveparams_t ¶ms, CPhysicsConstraint *pConstraint ); +bool RestorePhysicsConstraint( const physrestoreparams_t ¶ms, CPhysicsConstraint **ppConstraint ); + +bool SavePhysicsConstraintGroup( const physsaveparams_t ¶ms, CPhysicsConstraintGroup *pConstraintGroup ); +bool RestorePhysicsConstraintGroup( const physrestoreparams_t ¶ms, CPhysicsConstraintGroup **ppConstraintGroup ); +void PostRestorePhysicsConstraintGroup(); + +bool SavePhysicsShadowController( const physsaveparams_t ¶ms, IPhysicsShadowController *pShadowController ); +bool RestorePhysicsShadowController( const physrestoreparams_t ¶ms, IPhysicsShadowController **ppShadowController ); +bool RestorePhysicsShadowControllerInternal( const physrestoreparams_t ¶ms, IPhysicsShadowController **ppShadowController, CPhysicsObject *pObject ); + +bool SavePhysicsPlayerController( const physsaveparams_t ¶ms, CPlayerController *pPlayerController ); +bool RestorePhysicsPlayerController( const physrestoreparams_t ¶ms, CPlayerController **ppPlayerController ); + +bool SavePhysicsMotionController( const physsaveparams_t ¶ms, IPhysicsMotionController *pMotionController ); +bool RestorePhysicsMotionController( const physrestoreparams_t ¶ms, IPhysicsMotionController **ppMotionController ); + +bool SavePhysicsVehicleController( const physsaveparams_t ¶ms, CVehicleController *pVehicleController ); +bool RestorePhysicsVehicleController( const physrestoreparams_t ¶ms, CVehicleController **ppVehicleController ); + +//----------------------------------------------------------------------------- + +ISaveRestoreOps* MaterialIndexDataOps(); + +#endif // VPHYSICS_SAVERESTORE_H diff --git a/vphysics/xbox/xbox.def b/vphysics/xbox/xbox.def new file mode 100644 index 0000000..fe76a1f --- /dev/null +++ b/vphysics/xbox/xbox.def @@ -0,0 +1,3 @@ +LIBRARY vphysics_360.dll +EXPORTS + CreateInterface @1 |