384 lines
13 KiB
C++
384 lines
13 KiB
C++
//=================================================================================================
|
|
//
|
|
// Source / Jolt utilities
|
|
//
|
|
//=================================================================================================
|
|
|
|
#pragma once
|
|
|
|
inline constexpr float InchesToMetres = 0.0254f;
|
|
inline constexpr float MetresToInches = 1.0f / 0.0254f;
|
|
|
|
// TODO! Remove loam_expr -> constexpr when mathlib stuff is sorted.
|
|
#define loam_expr inline
|
|
|
|
loam_expr Vector VectorHalfExtent( Vector mins, Vector maxs )
|
|
{
|
|
return 0.5f * ( maxs - mins );
|
|
}
|
|
|
|
loam_expr Quaternion ToQuaternion( const QAngle& angles )
|
|
{
|
|
Quaternion result;
|
|
AngleQuaternion( angles, result );
|
|
return result;
|
|
}
|
|
|
|
loam_expr QAngle ToQAngle( const Quaternion &q )
|
|
{
|
|
QAngle result;
|
|
QuaternionAngles( q, result );
|
|
return result;
|
|
}
|
|
|
|
loam_expr QAngle ToQAngle( const matrix3x4_t& m )
|
|
{
|
|
QAngle result;
|
|
MatrixAngles( m, result );
|
|
return result;
|
|
}
|
|
|
|
loam_expr Vector Abs( const Vector &v )
|
|
{
|
|
Vector result;
|
|
VectorAbs( v, result );
|
|
return result;
|
|
}
|
|
|
|
loam_expr Vector Rotate( const Vector &vector, const Quaternion &angle )
|
|
{
|
|
Vector out;
|
|
VectorRotate( vector, angle, out );
|
|
return out;
|
|
}
|
|
|
|
loam_expr Vector Rotate( const Vector& vector, const matrix3x4_t &matrix )
|
|
{
|
|
Vector out;
|
|
VectorRotate( vector, matrix, out );
|
|
return out;
|
|
}
|
|
|
|
template < typename T >
|
|
constexpr T Cube( T x )
|
|
{
|
|
return x * x * x;
|
|
}
|
|
|
|
namespace MatrixAxis
|
|
{
|
|
enum JoltMatrixAxes
|
|
{
|
|
Forward = 0,
|
|
Left = 1,
|
|
Up = 2,
|
|
|
|
X = 0,
|
|
Y = 1,
|
|
Z = 2,
|
|
|
|
Origin = 3,
|
|
Projective = 3,
|
|
};
|
|
}
|
|
using JoltMatrixAxes = MatrixAxis::JoltMatrixAxes;
|
|
|
|
loam_expr Vector GetColumn( const matrix3x4_t& m, JoltMatrixAxes axis )
|
|
{
|
|
Vector value;
|
|
MatrixGetColumn( m, (int)axis, value );
|
|
return value;
|
|
}
|
|
|
|
//
|
|
// SourceToJolt:
|
|
//
|
|
// Type conversions from:
|
|
// - JPH::Vec3 -> Vector,
|
|
// - JPH::Float3 -> Vector,
|
|
// - JPH::Quat-> Quaternion,
|
|
// - JPH::Quat -> QAngle,
|
|
// Unit conversions from:
|
|
// - metres -> inches (distance, area, volume, energy)
|
|
// - radians -> degrees
|
|
namespace JoltToSource
|
|
{
|
|
inline constexpr float Factor = MetresToInches;
|
|
inline constexpr float InvFactor = InchesToMetres;
|
|
|
|
// This is for doing direct type conversions
|
|
// ie. normals, directions, scale factors, coefficients and
|
|
// certain dimensionless quantities.
|
|
loam_expr float Unitless( float value ) { return value; }
|
|
loam_expr Vector Unitless( JPH::Vec3Arg value ) { return Vector( value[0], value[1], value[2] ); }
|
|
loam_expr Vector Unitless( JPH::Float3 value ) { return Vector( value[0], value[1], value[2] ); }
|
|
|
|
// This is used for any unit that has a singular metre factor
|
|
// ie. distance (m), velocity (m/s), acceleration (m/s^2), force (kg m/s^2).
|
|
loam_expr float Distance( float value ) { return value * Factor; }
|
|
loam_expr Vector Distance( JPH::Vec3Arg value ) { return Vector( Distance( value[0] ), Distance( value[1] ), Distance( value[2] ) ); }
|
|
loam_expr Vector Distance( JPH::Float3 value ) { return Vector( Distance( value[0] ), Distance( value[1] ), Distance( value[2] ) ); }
|
|
|
|
// m^2 -> in^2
|
|
loam_expr float Area( float value ) { return value * Factor * Factor; }
|
|
// m^3 -> in^3
|
|
loam_expr float Volume( float value ) { return value * Factor * Factor * Factor; }
|
|
|
|
// These handle converting Quaternions/QAngles -> JPH::Quat,
|
|
// which is a direct type passthrough.
|
|
// They also handles converting scalar angle units, which is rad -> deg.
|
|
loam_expr Quaternion Quat( JPH::QuatArg value ) { return Quaternion( value.GetX(), value.GetY(), value.GetZ(), value.GetW() ); }
|
|
loam_expr float Angle( float value ) { return RAD2DEG( value ); }
|
|
loam_expr QAngle Angle( JPH::QuatArg value ) { return ToQAngle( Quat( value ) ); }
|
|
|
|
loam_expr float Energy( float value ) { return value / ( InvFactor * InvFactor ); }
|
|
|
|
// Converts types and handles the angle (rad -> deg) unit conversion.
|
|
loam_expr float AngularImpulse( float value ) { return Angle( value ); }
|
|
loam_expr Vector AngularImpulse( JPH::Vec3Arg value ) { return Vector( AngularImpulse( value[0] ), AngularImpulse( value[1] ), AngularImpulse( value[2] ) ); }
|
|
|
|
// Misc. AABB helpers.
|
|
loam_expr Vector AABBCenter( const JPH::AABox &aabox ) { return Distance( aabox.mMin + aabox.GetExtent() ); }
|
|
loam_expr Vector AABBHalfExtent( const JPH::AABox &aabox ) { return Distance( aabox.GetExtent() ); }
|
|
loam_expr void AABBBounds( const JPH::AABox &aabox, Vector &outMins, Vector &outMaxs )
|
|
{
|
|
outMins = AABBCenter( aabox ) - AABBHalfExtent( aabox );
|
|
outMaxs = AABBCenter( aabox ) + AABBHalfExtent( aabox );
|
|
}
|
|
|
|
loam_expr matrix3x4_t Matrix( JPH::Mat44Arg matrix )
|
|
{
|
|
return matrix3x4_t
|
|
{
|
|
Vector( matrix.GetAxisX().GetX(), matrix.GetAxisX().GetY(), matrix.GetAxisX().GetZ() ),
|
|
Vector( matrix.GetAxisY().GetX(), matrix.GetAxisY().GetY(), matrix.GetAxisY().GetZ() ),
|
|
Vector( matrix.GetAxisZ().GetX(), matrix.GetAxisZ().GetY(), matrix.GetAxisZ().GetZ() ),
|
|
Distance( matrix.GetTranslation() ),
|
|
};
|
|
}
|
|
|
|
loam_expr ::Color Color( JPH::ColorArg color )
|
|
{
|
|
return ::Color( color.r, color.g, color.b, color.a );
|
|
}
|
|
}
|
|
|
|
//
|
|
// SourceToJolt:
|
|
//
|
|
// Type conversions from:
|
|
// - Vector -> JPH::Vec3,
|
|
// - Vector -> JPH::Float3,
|
|
// - Quaternion -> JPH::Quat,
|
|
// - QAngle -> JPH::Quat,
|
|
// Unit conversions from:
|
|
// - inches -> metres (distance, area, volume, energy)
|
|
// - degrees -> radians
|
|
namespace SourceToJolt
|
|
{
|
|
inline constexpr float Factor = InchesToMetres;
|
|
inline constexpr float InvFactor = MetresToInches;
|
|
|
|
// This is for doing direct type conversions
|
|
// ie. normals, directions, scale factors, coefficients and
|
|
// certain dimensionless quantities.
|
|
loam_expr float Unitless( float value ) { return value; }
|
|
loam_expr JPH::Vec3 Unitless( Vector value ) { return JPH::Vec3( value[0], value[1], value[2] ); }
|
|
loam_expr JPH::Float3 UnitlessFloat3( Vector value ) { return JPH::Float3( value[0], value[1], value[2] ); }
|
|
|
|
// This is used for any unit that has a singular Source Unit(tm) "inch" factor
|
|
// ie. distance (in), velocity (in/s), acceleration (in/s^2), force (kg in/s^2).
|
|
loam_expr float Distance( float value ) { return value * Factor; }
|
|
loam_expr JPH::Vec3 Distance( Vector value ) { return JPH::Vec3( Distance( value[0] ), Distance( value[1] ), Distance( value[2] ) ); }
|
|
loam_expr JPH::Float3 DistanceFloat3( Vector value ) { return JPH::Float3( Distance( value[0] ), Distance( value[1] ), Distance( value[2] ) ); }
|
|
|
|
// in^2 -> m^2
|
|
loam_expr float Area( float value ) { return value * Factor * Factor; }
|
|
// in^3 -> m^3
|
|
loam_expr float Volume( float value ) { return value * Factor * Factor * Factor; }
|
|
|
|
// These handle converting JPH::Quat -> Quaternions/QAngles,
|
|
// which is a direct type passthrough.
|
|
// They also handles converting scalar angle units, which is deg -> rad.
|
|
loam_expr JPH::Quat Quat( Quaternion value ) { return JPH::Quat( value.x, value.y, value.z, value.w ); }
|
|
loam_expr float Angle( float value ) { return DEG2RAD( value ); }
|
|
loam_expr JPH::Quat Angle( QAngle value ) { return Quat( ToQuaternion( value ) ); }
|
|
|
|
loam_expr float Energy( float value ) { return value / ( InvFactor * InvFactor ); }
|
|
|
|
// Converts types and handles the angle (deg -> rad) unit conversion.
|
|
loam_expr float AngularImpulse( float value ) { return Angle( value ); }
|
|
loam_expr JPH::Vec3 AngularImpulse( Vector value ) { return JPH::Vec3( AngularImpulse( value[0] ), AngularImpulse( value[1] ), AngularImpulse( value[2] ) ); }
|
|
|
|
// Misc. AABB helpers.
|
|
loam_expr JPH::Vec3 AABBCenter( Vector mins, Vector maxs ) { return Distance( mins + VectorHalfExtent( mins, maxs ) ); }
|
|
loam_expr JPH::Vec3 AABBHalfExtent( Vector mins, Vector maxs ) { return Distance( VectorHalfExtent( mins, maxs ) ); }
|
|
loam_expr JPH::AABox AABBBounds( Vector mins, Vector maxs )
|
|
{
|
|
return JPH::AABox
|
|
{
|
|
AABBCenter( mins, maxs ) - AABBHalfExtent( mins, maxs ),
|
|
AABBCenter( mins, maxs ) + AABBHalfExtent( mins, maxs ),
|
|
};
|
|
}
|
|
|
|
loam_expr JPH::Mat44 Matrix( const matrix3x4_t &m )
|
|
{
|
|
return JPH::Mat44
|
|
{
|
|
JPH::Vec4( GetColumn( m, MatrixAxis::X ).x, GetColumn( m, MatrixAxis::X ).y, GetColumn( m, MatrixAxis::X ).z, 0.0f ),
|
|
JPH::Vec4( GetColumn( m, MatrixAxis::Y ).x, GetColumn( m, MatrixAxis::Y ).y, GetColumn( m, MatrixAxis::Y ).z, 0.0f ),
|
|
JPH::Vec4( GetColumn( m, MatrixAxis::Z ).x, GetColumn( m, MatrixAxis::Z ).y, GetColumn( m, MatrixAxis::Z ).z, 0.0f ),
|
|
JPH::Vec4( Distance( GetColumn( m, MatrixAxis::Origin ) ), 1.0f ),
|
|
};
|
|
}
|
|
}
|
|
|
|
// Traces
|
|
|
|
// Same as CM_ClearTrace
|
|
inline void ClearTrace( trace_t *trace )
|
|
{
|
|
memset( trace, 0, sizeof( *trace ) );
|
|
trace->fraction = 1.0f;
|
|
trace->fractionleftsolid = 0.0f;
|
|
trace->surface.name = "**empty**";
|
|
}
|
|
|
|
// Converts a JoltPhysics smart ref-counted pointer to a raw pointer with a dangling
|
|
// reference that we can clean up explicitly explcitly later in eg. ConvexFree.
|
|
//
|
|
// The main reason is to avoids trailing class pointers that just have a single instance
|
|
// of this class that then get deleted, when we can just pass the raw pointer around ourselves
|
|
// and explicitly dereference on ourside when the game calls delete.
|
|
template < typename T >
|
|
T *ToDanglingRef( const JPH::Ref< T >& ref )
|
|
{
|
|
T *pPtr = ref.GetPtr();
|
|
pPtr->AddRef();
|
|
return pPtr;
|
|
}
|
|
|
|
template < typename T >
|
|
bool VectorContains( const std::vector< T >& vector, const T &object )
|
|
{
|
|
return std::find(vector.begin(), vector.end(), object) != vector.end();
|
|
}
|
|
|
|
inline const JPH::Shape* UndecorateShape( const JPH::Shape *pShape )
|
|
{
|
|
if ( pShape->GetType() == JPH::EShapeType::Decorated )
|
|
pShape = static_cast< const JPH::DecoratedShape * >( pShape )->GetInnerShape();
|
|
|
|
return pShape;
|
|
}
|
|
|
|
inline const JPH::StaticCompoundShape *GetCompoundShape( const JPH::Shape *pShape )
|
|
{
|
|
pShape = UndecorateShape( pShape );
|
|
|
|
return pShape->GetType() == JPH::EShapeType::Compound
|
|
? static_cast< const JPH::StaticCompoundShape * >( pShape )
|
|
: nullptr;
|
|
}
|
|
|
|
template < typename T, typename ShapeType, typename Func >
|
|
T ActOnSubShape( const JPH::Shape *pShape, int nIndex, Func ShapeFunc )
|
|
{
|
|
const JPH::StaticCompoundShape *pCompoundShape = GetCompoundShape( pShape );
|
|
if ( pCompoundShape )
|
|
{
|
|
const JPH::CompoundShape::SubShape& subShape = pCompoundShape->GetSubShape( nIndex );
|
|
return ShapeFunc( static_cast< const ShapeType * >( UndecorateShape( subShape.mShape.GetPtr() ) ) );
|
|
}
|
|
|
|
return ShapeFunc( static_cast< const ShapeType * >( UndecorateShape( pShape ) ) );
|
|
}
|
|
|
|
template < typename ShapeType, typename Func >
|
|
void ActOnSubShapes( const JPH::Shape *pShape, Func ShapeFunc )
|
|
{
|
|
const JPH::StaticCompoundShape *pCompoundShape = GetCompoundShape( pShape );
|
|
if ( pCompoundShape )
|
|
{
|
|
for ( const JPH::CompoundShape::SubShape& subShape : pCompoundShape->GetSubShapes() )
|
|
{
|
|
JPH::Mat44 matLocalTranslation = JPH::Mat44::sRotationTranslation( subShape.GetRotation(), subShape.GetPositionCOM() );
|
|
ShapeFunc( static_cast< const ShapeType * >( UndecorateShape( subShape.mShape.GetPtr() ) ), matLocalTranslation );
|
|
}
|
|
return;
|
|
}
|
|
|
|
ShapeFunc( static_cast< const ShapeType * >( UndecorateShape( pShape ) ), JPH::Mat44::sIdentity() );
|
|
}
|
|
|
|
inline uint32 popcntStep( uint32 n, uint32 mask, uint32 shift )
|
|
{
|
|
return ( n & mask ) + ( ( n & ~mask ) >> shift );
|
|
}
|
|
|
|
inline uint32 popcnt( uint32 n )
|
|
{
|
|
n = popcntStep(n, 0x55555555, 1);
|
|
n = popcntStep(n, 0x33333333, 2);
|
|
n = popcntStep(n, 0x0F0F0F0F, 4);
|
|
n = popcntStep(n, 0x00FF00FF, 8);
|
|
n = popcntStep(n, 0x0000FFFF, 16);
|
|
return n;
|
|
}
|
|
|
|
inline uint32 tzcnt( uint32 n )
|
|
{
|
|
#if defined(_MSC_VER) && !defined(__clang__)
|
|
return _tzcnt_u32( n );
|
|
#elif defined(__BMI__)
|
|
return __tzcnt_u32( n );
|
|
#elif defined(__GNUC__) || defined(__clang__)
|
|
// tzcnt is encoded as rep bsf, so we can use it on all
|
|
// processors, but the behaviour of zero inputs differs:
|
|
// - bsf: zf = 1, cf = ?, result = ?
|
|
// - tzcnt: zf = 0, cf = 1, result = 32
|
|
// We'll have to handle this case manually.
|
|
uint32 res;
|
|
uint32 tmp;
|
|
asm (
|
|
"tzcnt %2, %0;"
|
|
"mov $32, %1;"
|
|
"test %2, %2;"
|
|
"cmovz %1, %0;"
|
|
: "=&r" (res), "=&r" (tmp)
|
|
: "r" (n)
|
|
: "cc");
|
|
return res;
|
|
#else
|
|
uint32 r = 31;
|
|
n &= -n;
|
|
r -= ( n & 0x0000FFFF ) ? 16 : 0;
|
|
r -= ( n & 0x00FF00FF ) ? 8 : 0;
|
|
r -= ( n & 0x0F0F0F0F ) ? 4 : 0;
|
|
r -= ( n & 0x33333333 ) ? 2 : 0;
|
|
r -= ( n & 0x55555555 ) ? 1 : 0;
|
|
return n != 0 ? r : 32;
|
|
#endif
|
|
}
|
|
|
|
template< typename T, typename Value >
|
|
constexpr void Erase( T &c, const Value &value )
|
|
{
|
|
auto it = std::remove( c.begin(), c.end(), value );
|
|
c.erase( it, c.end() );
|
|
}
|
|
|
|
template< typename T, typename Pred >
|
|
constexpr void EraseIf( T &c, Pred pred )
|
|
{
|
|
auto it = std::remove_if( c.begin(), c.end(), pred );
|
|
c.erase( it, c.end() );
|
|
}
|
|
|
|
template< typename T, typename Value >
|
|
constexpr bool Contains( const T &c, const Value &value )
|
|
{
|
|
return c.find( value ) != c.end();
|
|
}
|