//-----------------------------------------------------------------------------
// Purpose: Rotates a matrix by 90 degrees in the plane of axis0/axis1
// Input : &matrix -
// axis0 -
// axis1 -
// Output : static void
//-----------------------------------------------------------------------------
static void MatrixRot90( matrix3x4_t &matrix, int axis0, int axis1 )
{
Vector col0, col1;
MatrixGetColumn( matrix, axis0, col0 );
MatrixGetColumn( matrix, axis1, col1 );
MatrixSetColumn( col1, axis0, matrix );
MatrixSetColumn( -col0, axis1, matrix );
}
//-----------------------------------------------------------------------------
// Updates the relative orientation of the camera, spring mode
//-----------------------------------------------------------------------------
void CWeaponIFMSteadyCam::UpdateDirectRelativeOrientation()
{
// Compute a player to steadycam matrix
VMatrix steadyCamToPlayer;
MatrixFromAngles( m_angRelativeAngles, steadyCamToPlayer );
MatrixSetColumn( steadyCamToPlayer, 3, m_vecRelativePosition );
// Compute a forward direction
Vector vecCurrentForward;
MatrixGetColumn( steadyCamToPlayer, 0, &vecCurrentForward );
// Before any updating occurs, sample the current
// world-space direction of the mouse
Vector vecDesiredDirection;
ComputeMouseRay( steadyCamToPlayer, vecDesiredDirection );
// rebuild a roll-less orientation based on that direction vector
matrix3x4_t mat;
MatrixFromForwardDirection( vecDesiredDirection, mat );
MatrixAngles( mat, m_angRelativeAngles );
Assert( m_angRelativeAngles.IsValid() );
m_vecActualViewOffset -= m_vecViewOffset;
m_vecViewOffset.Init();
}
static QAngle AlignAngles( const QAngle &angles, float cosineAlignAngle )
{
matrix3x4_t alignMatrix;
AngleMatrix( angles, alignMatrix );
// NOTE: Must align z first
for ( int j = 3; --j >= 0; )
{
Vector vec;
MatrixGetColumn( alignMatrix, j, vec );
for ( int i = 0; i < 3; i++ )
{
if ( fabs(vec[i]) > cosineAlignAngle )
{
vec[i] = SIGN(vec[i]);
vec[(i+1)%3] = 0;
vec[(i+2)%3] = 0;
MatrixSetColumn( vec, j, alignMatrix );
MatrixOrthogonalize( alignMatrix, j );
break;
}
}
}
QAngle out;
MatrixAngles( alignMatrix, out );
return out;
}
//-----------------------------------------------------------------------------
// Returns the unperterbed view position for a particular role
//-----------------------------------------------------------------------------
bool CBaseTFVehicle::GetRoleViewPosition( int nRole, Vector *pVehicleEyeOrigin, QAngle *pVehicleEyeAngles )
{
// Generate the view position in world space.
Vector vAbsOrigin;
QAngle vAbsAngle;
bool bUsingThirdPersonCamera = GetRoleAbsViewPosition( nRole, &vAbsOrigin, &vAbsAngle );
// Make a matrix for it.
matrix3x4_t absMatrix;
AngleMatrix( vAbsAngle, absMatrix );
MatrixSetColumn( vAbsOrigin, 3, absMatrix );
// Transform the matrix into local space.
matrix3x4_t worldToEntity, local;
MatrixInvert( EntityToWorldTransform(), worldToEntity );
ConcatTransforms( worldToEntity, absMatrix, local );
// Suck out the origin and angles.
pVehicleEyeOrigin->Init( local[0][3], local[1][3], local[2][3] );
MatrixAngles( local, *pVehicleEyeAngles );
return bUsingThirdPersonCamera;
}
void C_NPC_Hydra::CalcBoneAngles( const Vector pos[], Quaternion q[] )
{
int i;
matrix3x4_t bonematrix;
for (i = m_numHydraBones - 1; i >= 0; i--)
{
Vector forward;
Vector left2;
if (i != m_numHydraBones - 1)
{
QuaternionMatrix( q[i+1], bonematrix );
MatrixGetColumn( bonematrix, 1, left2 );
forward = (pos[i+1] - pos[i]) /* + (pos[i] - pos[i-1])*/;
float length = VectorNormalize( forward );
if (length == 0.0)
{
q[i] = q[i+1];
continue;
}
}
else
{
forward = m_vecHeadDir;
VectorNormalize( forward );
VectorMatrix( forward, bonematrix );
MatrixGetColumn( bonematrix, 1, left2 );
}
Vector up = CrossProduct( forward, left2 );
VectorNormalize( up );
Vector left = CrossProduct( up, forward );
MatrixSetColumn( forward, 0, bonematrix );
MatrixSetColumn( left, 1, bonematrix );
MatrixSetColumn( up, 2, bonematrix );
// MatrixQuaternion( bonematrix, q[i] );
QAngle angles;
MatrixAngles( bonematrix, angles );
AngleQuaternion( angles, q[i] );
}
}
void CRagdollPropAttached::InitRagdollAttached( IPhysicsObject *pAttached, const Vector &forceVector, int forceBone, matrix3x4_t *pPrevBones, matrix3x4_t *pBoneToWorld, float dt, int collisionGroup, CBaseAnimating *pFollow, int boneIndexRoot, const Vector &boneLocalOrigin, int parentBoneAttach, const Vector &worldAttachOrigin )
{
int ragdollAttachedIndex = 0;
if ( parentBoneAttach > 0 )
{
studiohdr_t *pStudioHdr = GetModelPtr();
mstudiobone_t *pBone = pStudioHdr->pBone( parentBoneAttach );
ragdollAttachedIndex = pBone->physicsbone;
}
InitRagdoll( forceVector, forceBone, vec3_origin, pPrevBones, pBoneToWorld, dt, collisionGroup, false );
IPhysicsObject *pRefObject = m_ragdoll.list[ragdollAttachedIndex].pObject;
Vector attachmentPointRagdollSpace;
pRefObject->WorldToLocal( attachmentPointRagdollSpace, worldAttachOrigin );
constraint_ragdollparams_t constraint;
constraint.Defaults();
matrix3x4_t tmp, worldToAttached, worldToReference, constraintToWorld;
Vector offsetWS;
pAttached->LocalToWorld( offsetWS, boneLocalOrigin );
AngleMatrix( QAngle(0, pFollow->GetAbsAngles().y, 0 ), offsetWS, constraintToWorld );
constraint.axes[0].SetAxisFriction( -2, 2, 20 );
constraint.axes[1].SetAxisFriction( 0, 0, 0 );
constraint.axes[2].SetAxisFriction( -15, 15, 20 );
pAttached->GetPositionMatrix( tmp );
MatrixInvert( tmp, worldToAttached );
pRefObject->GetPositionMatrix( tmp );
MatrixInvert( tmp, worldToReference );
ConcatTransforms( worldToReference, constraintToWorld, constraint.constraintToReference );
ConcatTransforms( worldToAttached, constraintToWorld, constraint.constraintToAttached );
// for now, just slam this to be the passed in value
MatrixSetColumn( attachmentPointRagdollSpace, 3, constraint.constraintToReference );
DisableCollisions( pAttached );
m_pAttachConstraint = physenv->CreateRagdollConstraint( pRefObject, pAttached, m_ragdoll.pGroup, constraint );
FollowEntity( pFollow );
SetOwnerEntity( pFollow );
RagdollActivate( m_ragdoll );
Relink();
m_boneIndexAttached = boneIndexRoot;
m_ragdollAttachedObjectIndex = ragdollAttachedIndex;
m_attachmentPointBoneSpace = boneLocalOrigin;
Vector vTemp;
MatrixGetColumn( constraint.constraintToReference, 3, vTemp );
m_attachmentPointRagdollSpace = vTemp;
}
//-----------------------------------------------------------------------------------
void Matrix4x4::MatrixInvertOrthogonal(Matrix4x4* matrix)
{
Vector3 translation = MatrixGetOffset(matrix);
MatrixTranspose(matrix);
MatrixSetColumn(matrix, 3, Vector4(0.0f, 0.0f, 0.0f, 1.0f));
Matrix4x4 translationMatrix;
MatrixMakeTranslation(&translationMatrix, translation);
MatrixInvert(&translationMatrix);
MatrixMultiply(matrix, &translationMatrix, matrix);
}
static void MatrixOrthogonalize( matrix3x4_t &matrix, int column )
{
Vector columns[3];
int i;
for ( i = 0; i < 3; i++ )
{
MatrixGetColumn( matrix, i, columns[i] );
}
int index0 = column;
int index1 = (column+1)%3;
int index2 = (column+2)%3;
columns[index2] = CrossProduct( columns[index0], columns[index1] );
columns[index1] = CrossProduct( columns[index2], columns[index0] );
VectorNormalize( columns[index2] );
VectorNormalize( columns[index1] );
MatrixSetColumn( columns[index1], index1, matrix );
MatrixSetColumn( columns[index2], index2, matrix );
}
//-----------------------------------------------------------------------------
// Apply movement
//-----------------------------------------------------------------------------
void CLogicMeasureMovement::MeasureThink( )
{
// FIXME: This is a hack to make measuring !player simpler. The player isn't
// created at Activate time, so m_hMeasureTarget may be NULL because of that.
if ( !m_hMeasureTarget.Get() && !Q_strnicmp( STRING(m_strMeasureTarget), "!player", 8 ) )
{
SetMeasureTarget( STRING(m_strMeasureTarget) );
}
// Make sure all entities are valid
if ( m_hMeasureTarget.Get() && m_hMeasureReference.Get() && m_hTarget.Get() && m_hTargetReference.Get() )
{
matrix3x4_t matRefToMeasure, matWorldToMeasure;
switch( m_nMeasureType )
{
case MEASURE_POSITION:
MatrixInvert( m_hMeasureTarget->EntityToWorldTransform(), matWorldToMeasure );
break;
case MEASURE_EYE_POSITION:
AngleIMatrix( m_hMeasureTarget->EyeAngles(), m_hMeasureTarget->EyePosition(), matWorldToMeasure );
break;
// FIXME: Could add attachment point measurement here easily
}
ConcatTransforms( matWorldToMeasure, m_hMeasureReference->EntityToWorldTransform(), matRefToMeasure );
// Apply the scale factor
if ( ( m_flScale != 0.0f ) && ( m_flScale != 1.0f ) )
{
Vector vecTranslation;
MatrixGetColumn( matRefToMeasure, 3, vecTranslation );
vecTranslation /= m_flScale;
MatrixSetColumn( vecTranslation, 3, matRefToMeasure );
}
// Now apply the new matrix to the new reference point
matrix3x4_t matMeasureToRef, matNewTargetToWorld;
MatrixInvert( matRefToMeasure, matMeasureToRef );
ConcatTransforms( m_hTargetReference->EntityToWorldTransform(), matMeasureToRef, matNewTargetToWorld );
Vector vecNewOrigin;
QAngle vecNewAngles;
MatrixAngles( matNewTargetToWorld, vecNewAngles, vecNewOrigin );
m_hTarget->SetAbsOrigin( vecNewOrigin );
m_hTarget->SetAbsAngles( vecNewAngles );
}
SetNextThink( gpGlobals->curtime + TICK_INTERVAL );
}
virtual void BuildTransformations( CStudioHdr *hdr, Vector *pos, Quaternion q[], const matrix3x4_t& cameraTransform, int boneMask, CBoneBitList &boneComputed )
{
VPROF_BUDGET( "C_ServerRagdollAttached::SetupBones", VPROF_BUDGETGROUP_CLIENT_ANIMATION );
if ( !hdr )
return;
float frac = RemapVal( gpGlobals->curtime, m_parentTime, m_parentTime+ATTACH_INTERP_TIME, 0, 1 );
frac = clamp( frac, 0.f, 1.f );
// interpolate offset over some time
Vector offset = m_vecOffset * (1-frac);
C_BaseAnimating *parent = assert_cast< C_BaseAnimating* >( GetMoveParent() );
Vector worldOrigin;
worldOrigin.Init();
if ( parent )
{
Assert( parent != this );
parent->SetupBones( NULL, -1, BONE_USED_BY_ANYTHING, gpGlobals->curtime );
matrix3x4_t boneToWorld;
parent->GetCachedBoneMatrix( m_boneIndexAttached, boneToWorld );
VectorTransform( m_attachmentPointBoneSpace, boneToWorld, worldOrigin );
}
BaseClass::BuildTransformations( hdr, pos, q, cameraTransform, boneMask, boneComputed );
if ( parent )
{
int index = m_boneIndex[m_ragdollAttachedObjectIndex];
const matrix3x4_t &matrix = GetBone( index );
Vector ragOrigin;
VectorTransform( m_attachmentPointRagdollSpace, matrix, ragOrigin );
offset = worldOrigin - ragOrigin;
// fixes culling
SetAbsOrigin( worldOrigin );
m_vecOffset = offset;
}
for ( int i = 0; i < hdr->numbones(); i++ )
{
if ( !( hdr->boneFlags( i ) & boneMask ) )
continue;
Vector pos;
matrix3x4_t &matrix = GetBoneForWrite( i );
MatrixGetColumn( matrix, 3, pos );
pos += offset;
MatrixSetColumn( pos, 3, matrix );
}
}
const matrix3x4_t& CCollisionProperty::CollisionToWorldTransform() const
{
static matrix3x4_t s_matTemp[4];
static int s_nIndex = 0;
matrix3x4_t &matResult = s_matTemp[s_nIndex];
s_nIndex = (s_nIndex+1) & 0x3;
if ( IsBoundsDefinedInEntitySpace() )
{
return m_pOuter->EntityToWorldTransform();
}
SetIdentityMatrix( matResult );
MatrixSetColumn( GetCollisionOrigin(), 3, matResult );
return matResult;
}
//-----------------------------------------------------------------------------
// Does a fast inverse, assuming the matrix only contains translation and rotation.
//-----------------------------------------------------------------------------
void MatrixInverseTR( const VMatrix& src, VMatrix &dst )
{
Vector vTrans, vNewTrans;
// Transpose the upper 3x3.
dst.m[0][0] = src.m[0][0]; dst.m[0][1] = src.m[1][0]; dst.m[0][2] = src.m[2][0];
dst.m[1][0] = src.m[0][1]; dst.m[1][1] = src.m[1][1]; dst.m[1][2] = src.m[2][1];
dst.m[2][0] = src.m[0][2]; dst.m[2][1] = src.m[1][2]; dst.m[2][2] = src.m[2][2];
// Transform the translation.
vTrans.Init( -src.m[0][3], -src.m[1][3], -src.m[2][3] );
Vector3DMultiply( dst, vTrans, vNewTrans );
MatrixSetColumn( dst, 3, vNewTrans );
// Fill in the bottom row.
dst.m[3][0] = dst.m[3][1] = dst.m[3][2] = 0.0f;
dst.m[3][3] = 1.0f;
}
static void RagdollAddSolids( IPhysicsEnvironment *pPhysEnv, ragdoll_t &ragdoll, const ragdollparams_t ¶ms, cache_ragdollsolid_t *pSolids, int solidCount, const cache_ragdollconstraint_t *pConstraints, int constraintCount )
{
const char *pszName = params.pStudioHdr->pszName();
Vector position;
matrix3x4_t xform;
// init parent index
for ( int i = 0; i < solidCount; i++ )
{
ragdoll.list[i].parentIndex = -1;
}
// now set from constraints
for ( int i = 0; i < constraintCount; i++ )
{
// save parent index
ragdoll.list[pConstraints[i].childIndex].parentIndex = pConstraints[i].parentIndex;
MatrixGetColumn( pConstraints[i].constraintToAttached, 3, ragdoll.list[pConstraints[i].childIndex].originParentSpace );
}
// now setup the solids, using parent indices
for ( int i = 0; i < solidCount; i++ )
{
ragdoll.boneIndex[i] = pSolids[i].boneIndex;
pSolids[i].params.pName = pszName;
pSolids[i].params.pGameData = params.pGameData;
ragdoll.list[i].pObject = pPhysEnv->CreatePolyObject( params.pCollide->solids[pSolids[i].collideIndex], pSolids[i].surfacePropIndex, vec3_origin, vec3_angle, &pSolids[i].params );
ragdoll.list[i].pObject->SetGameIndex( i );
int parentIndex = ragdoll.list[i].parentIndex;
MatrixCopy( params.pCurrentBones[ragdoll.boneIndex[i]], xform );
if ( parentIndex >= 0 )
{
Assert(parentIndex<i);
ragdoll.list[parentIndex].pObject->LocalToWorld( &position, ragdoll.list[i].originParentSpace );
MatrixSetColumn( position, 3, xform );
}
ragdoll.list[i].pObject->SetPositionMatrix( xform, true );
PhysSetGameFlags( ragdoll.list[i].pObject, FVPHYSICS_PART_OF_RAGDOLL );
}
ragdoll.listCount = solidCount;
}
bool RagdollGetBoneMatrix( const ragdoll_t &ragdoll, matrix3x4_t *pBoneToWorld, int objectIndex )
{
int boneIndex = ragdoll.boneIndex[objectIndex];
if ( boneIndex < 0 )
return false;
const ragdollelement_t &element = ragdoll.list[objectIndex];
element.pObject->GetPositionMatrix( pBoneToWorld[boneIndex] );
if ( element.parentIndex >= 0 )
{
// overwrite the position from physics to force rigid attachment
// UNDONE: If we support other types of constraints (or multiple constraints per object)
// make sure these don't fight !
int parentBoneIndex = ragdoll.boneIndex[element.parentIndex];
Vector out;
VectorTransform( element.originParentSpace, pBoneToWorld[parentBoneIndex], out );
MatrixSetColumn( out, 3, pBoneToWorld[boneIndex] );
}
return true;
}
bool RagdollGetBoneMatrix( const ragdoll_t &ragdoll, CBoneAccessor &pBoneToWorld, int objectIndex )
{
int boneIndex = ragdoll.boneIndex[objectIndex];
if ( boneIndex < 0 )
return false;
const ragdollelement_t &element = ragdoll.list[objectIndex];
// during restore if a model has changed since the file was saved, this could be NULL
if ( !element.pObject )
return false;
element.pObject->GetPositionMatrix( &pBoneToWorld.GetBoneForWrite( boneIndex ) );
if ( element.parentIndex >= 0 && !ragdoll.allowStretch )
{
// overwrite the position from physics to force rigid attachment
// NOTE: On the client we actually override this with the proper parent bone in each LOD
int parentBoneIndex = ragdoll.boneIndex[element.parentIndex];
Vector out;
VectorTransform( element.originParentSpace, pBoneToWorld.GetBone( parentBoneIndex ), out );
MatrixSetColumn( out, 3, pBoneToWorld.GetBoneForWrite( boneIndex ) );
}
return true;
}
bool RagdollGetBoneMatrix( const ragdoll_t &ragdoll, CBoneAccessor &pBoneToWorld, int objectIndex )
{
int boneIndex = ragdoll.boneIndex[objectIndex];
if ( boneIndex < 0 )
return false;
const ragdollelement_t &element = ragdoll.list[objectIndex];
// during restore if a model has changed since the file was saved, this could be NULL
if ( !element.pObject )
return false;
element.pObject->GetPositionMatrix( &pBoneToWorld.GetBoneForWrite( boneIndex ) );
if ( element.parentIndex >= 0 && !ragdoll.allowStretch )
{
// overwrite the position from physics to force rigid attachment
// UNDONE: If we support other types of constraints (or multiple constraints per object)
// make sure these don't fight !
int parentBoneIndex = ragdoll.boneIndex[element.parentIndex];
Vector out;
VectorTransform( element.originParentSpace, pBoneToWorld.GetBone( parentBoneIndex ), out );
MatrixSetColumn( out, 3, pBoneToWorld.GetBoneForWrite( boneIndex ) );
}
return true;
}
void RagdollSolveSeparation( ragdoll_t &ragdoll, CBaseEntity *pEntity )
{
byte needsFix[256];
int fixCount = 0;
Assert(ragdoll.listCount<=ARRAYSIZE(needsFix));
for ( int i = 0; i < ragdoll.listCount; i++ )
{
needsFix[i] = 0;
const ragdollelement_t &element = ragdoll.list[i];
if ( element.pConstraint && element.parentIndex >= 0 )
{
Vector start, target;
element.pObject->GetPosition( &start, NULL );
ragdoll.list[element.parentIndex].pObject->LocalToWorld( &target, element.originParentSpace );
if ( needsFix[element.parentIndex] )
{
needsFix[i] = 1;
++fixCount;
continue;
}
Vector dir = target-start;
if ( dir.LengthSqr() > 1.0f )
{
// this fixes a bug in ep2 with antlion grubs, but causes problems in TF2 - revisit, but disable for TF now
#if !defined(TF_CLIENT_DLL)
// heuristic: guess that anything separated and small mass ratio is in some state that's
// keeping the solver from fixing it
float mass = element.pObject->GetMass();
float massParent = ragdoll.list[element.parentIndex].pObject->GetMass();
if ( mass*2.0f < massParent )
{
// if this is <0.5 mass of parent and still separated it's attached to something heavy or
// in a bad state
needsFix[i] = 1;
++fixCount;
continue;
}
#endif
if ( PhysHasContactWithOtherInDirection(element.pObject, dir) )
{
Ray_t ray;
trace_t tr;
ray.Init( target, start );
UTIL_TraceRay( ray, MASK_SOLID, pEntity, COLLISION_GROUP_NONE, &tr );
if ( tr.DidHit() )
{
needsFix[i] = 1;
++fixCount;
}
}
}
}
}
if ( fixCount )
{
for ( int i = 0; i < ragdoll.listCount; i++ )
{
if ( !needsFix[i] )
continue;
const ragdollelement_t &element = ragdoll.list[i];
Vector target, velocity;
ragdoll.list[element.parentIndex].pObject->LocalToWorld( &target, element.originParentSpace );
ragdoll.list[element.parentIndex].pObject->GetVelocityAtPoint( target, &velocity );
matrix3x4_t xform;
element.pObject->GetPositionMatrix( &xform );
MatrixSetColumn( target, 3, xform );
element.pObject->SetPositionMatrix( xform, true );
element.pObject->SetVelocity( &velocity, &vec3_origin );
}
DevMsg(2, "TICK:%5d:Ragdoll separation count: %d\n", gpGlobals->tickcount, fixCount );
}
else
{
ragdoll.pGroup->ClearErrorState();
}
}
// apply custom pitch to bone merge
void CASW_Bone_Merge_Cache::MergeMatchingBones( int boneMask, CBoneBitList &boneComputed, bool bOverrideDirection, const Vector &vecDir )
{
UpdateCache();
// If this is set, then all the other cache data is set.
if ( !m_pOwnerHdr || m_MergedBones.Count() == 0 )
return;
// Have the entity we're following setup its bones.
m_pFollow->SetupBones( NULL, -1, m_nFollowBoneSetupMask, gpGlobals->curtime );
matrix3x4_t matPitchUp;
AngleMatrix( QAngle( asw_weapon_pitch.GetFloat(), 0, 0 ), matPitchUp );
// Now copy the bone matrices.
for ( int i=0; i < m_MergedBones.Count(); i++ )
{
int iOwnerBone = m_MergedBones[i].m_iMyBone;
int iParentBone = m_MergedBones[i].m_iParentBone;
// Only update bones reference by the bone mask.
if ( !( m_pOwnerHdr->boneFlags( iOwnerBone ) & boneMask ) )
continue;
if ( bOverrideDirection && m_nRightHandBoneID == -1 ) // only want to change direction of the right hand bone, cache its index here
{
mstudiobone_t *pOwnerBones = m_pOwnerHdr->pBone( 0 );
for ( int k = 0; k < m_pOwnerHdr->numbones(); k++ )
{
if ( !Q_stricmp( pOwnerBones[k].pszName(), "ValveBiped.Bip01_R_Hand" ) )
{
m_nRightHandBoneID = k;
break;
}
}
}
if ( bOverrideDirection && i == m_nRightHandBoneID )
{
matrix3x4_t matParentBoneToWorld;
m_pFollow->GetBoneTransform( iParentBone, matParentBoneToWorld );
MatrixSetColumn( vec3_origin, 3, matParentBoneToWorld ); // remove translation
matrix3x4_t matParentBoneToWorldInv;
MatrixInvert( matParentBoneToWorld, matParentBoneToWorldInv );
QAngle angAiming;
VectorAngles( vecDir, Vector( 0, 0, -1 ), angAiming );
matrix3x4_t matAimDirection;
AngleMatrix( angAiming, matAimDirection );
MatrixSetColumn( vec3_origin, 3, matAimDirection ); // remove translation
matrix3x4_t matCorrection;
ConcatTransforms( matParentBoneToWorldInv, matAimDirection, matCorrection );
ConcatTransforms( m_pFollow->GetBone( iParentBone ), matCorrection, m_pOwner->GetBoneForWrite( iOwnerBone ) );
}
else
{
ConcatTransforms( m_pFollow->GetBone( iParentBone ), matPitchUp, m_pOwner->GetBoneForWrite( iOwnerBone ) );
}
boneComputed.Set( i );
}
}
CBaseEntity *CreateServerRagdoll( CBaseAnimating *pAnimating, int forceBone, const CTakeDamageInfo &info, int collisionGroup, bool bUseLRURetirement )
{
SyncAnimatingWithPhysics( pAnimating );
CRagdollProp *pRagdoll = (CRagdollProp *)CBaseEntity::CreateNoSpawn( "prop_ragdoll", pAnimating->GetAbsOrigin(), vec3_angle, NULL );
pRagdoll->CopyAnimationDataFrom( pAnimating );
pRagdoll->SetOwnerEntity( pAnimating );
pRagdoll->InitRagdollAnimation();
matrix3x4_t pBoneToWorld[MAXSTUDIOBONES], pBoneToWorldNext[MAXSTUDIOBONES];
const float dt = 0.1f;
// Copy over dissolve state...
if ( pAnimating->IsEFlagSet( EFL_NO_DISSOLVE ) )
{
pRagdoll->AddEFlags( EFL_NO_DISSOLVE );
}
// NOTE: This currently is only necessary to prevent manhacks from
// colliding with server ragdolls they kill
pRagdoll->SetKiller( info.GetInflictor() );
pRagdoll->SetSourceClassName( pAnimating->GetClassname() );
// NPC_STATE_DEAD npc's will have their COND_IN_PVS cleared, so this needs to force SetupBones to happen
unsigned short fPrevFlags = pAnimating->GetBoneCacheFlags();
pAnimating->SetBoneCacheFlags( BCF_NO_ANIMATION_SKIP );
// UNDONE: Extract velocity from bones via animation (like we do on the client)
// UNDONE: For now, just move each bone by the total entity velocity if set.
pAnimating->SetupBones( pBoneToWorld, BONE_USED_BY_ANYTHING );
// Reset previous bone flags
pAnimating->ClearBoneCacheFlags( BCF_NO_ANIMATION_SKIP );
pAnimating->SetBoneCacheFlags( fPrevFlags );
memcpy( pBoneToWorldNext, pBoneToWorld, sizeof(pBoneToWorld) );
Vector vel = pAnimating->GetAbsVelocity();
if ( vel.LengthSqr() > 0 )
{
int numbones = pAnimating->GetModelPtr()->numbones();
for ( int i = 0; i < numbones; i++ )
{
Vector pos;
MatrixGetColumn( pBoneToWorldNext[i], 3, pos );
pos += vel * dt;
MatrixSetColumn( pos, 3, pBoneToWorldNext[i] );
}
}
// Is this a vehicle / NPC collision?
if ( (info.GetDamageType() & DMG_VEHICLE) && pAnimating->MyNPCPointer() )
{
// init the ragdoll with no forces
pRagdoll->InitRagdoll( vec3_origin, -1, vec3_origin, pBoneToWorld, pBoneToWorldNext, dt, collisionGroup, true );
// apply vehicle forces
// Get a list of bones with hitboxes below the plane of impact
int boxList[128];
Vector normal(0,0,-1);
int count = pAnimating->GetHitboxesFrontside( boxList, ARRAYSIZE(boxList), normal, DotProduct( normal, info.GetDamagePosition() ) );
// distribute force over mass of entire character
float massScale = Studio_GetMass(pAnimating->GetModelPtr());
massScale = clamp( massScale, 1, 1e4 );
massScale = 1 / massScale;
// distribute the force
// BUGBUG: This will hit the same bone twice if it has two hitboxes!!!!
ragdoll_t *pRagInfo = pRagdoll->GetRagdoll();
for ( int i = 0; i < count; i++ )
{
int physBone = pAnimating->GetPhysicsBone( pAnimating->GetHitboxBone( boxList[i] ) );
IPhysicsObject *pPhysics = pRagInfo->list[physBone].pObject;
pPhysics->ApplyForceCenter( info.GetDamageForce() * pPhysics->GetMass() * massScale );
}
}
else
{
pRagdoll->InitRagdoll( info.GetDamageForce(), forceBone, info.GetDamagePosition(), pBoneToWorld, pBoneToWorldNext, dt, collisionGroup, true );
}
// Are we dissolving?
if ( pAnimating->IsDissolving() )
{
pRagdoll->TransferDissolveFrom( pAnimating );
}
else if ( bUseLRURetirement )
{
pRagdoll->AddSpawnFlags( SF_RAGDOLLPROP_USE_LRU_RETIREMENT );
s_RagdollLRU.MoveToTopOfLRU( pRagdoll );
}
// Tracker 22598: If we don't set the OBB mins/maxs to something valid here, then the client will have a zero sized hull
// for the ragdoll for one frame until Vphysics updates the real obb bounds after the first simulation frame. Having
// a zero sized hull makes the ragdoll think it should be faded/alpha'd to zero for a frame, so you get a blink where
// the ragdoll doesn't draw initially.
Vector mins, maxs;
mins = pAnimating->CollisionProp()->OBBMins();
maxs = pAnimating->CollisionProp()->OBBMaxs();
pRagdoll->CollisionProp()->SetCollisionBounds( mins, maxs );
return pRagdoll;
}
void CRagdollPropAttached::InitRagdollAttached(
IPhysicsObject *pAttached,
const Vector &forceVector,
int forceBone,
matrix3x4_t *pPrevBones,
matrix3x4_t *pBoneToWorld,
float dt,
int collisionGroup,
CBaseAnimating *pFollow,
int boneIndexRoot,
const Vector &boneLocalOrigin,
int parentBoneAttach,
const Vector &worldAttachOrigin )
{
int ragdollAttachedIndex = 0;
if ( parentBoneAttach > 0 )
{
CStudioHdr *pStudioHdr = GetModelPtr();
mstudiobone_t *pBone = pStudioHdr->pBone( parentBoneAttach );
ragdollAttachedIndex = pBone->physicsbone;
}
InitRagdoll( forceVector, forceBone, vec3_origin, pPrevBones, pBoneToWorld, dt, collisionGroup, false );
IPhysicsObject *pRefObject = m_ragdoll.list[ragdollAttachedIndex].pObject;
Vector attachmentPointRagdollSpace;
pRefObject->WorldToLocal( &attachmentPointRagdollSpace, worldAttachOrigin );
constraint_ragdollparams_t constraint;
constraint.Defaults();
matrix3x4_t tmp, worldToAttached, worldToReference, constraintToWorld;
Vector offsetWS;
pAttached->LocalToWorld( &offsetWS, boneLocalOrigin );
AngleMatrix( QAngle(0, pFollow->GetAbsAngles().y, 0 ), offsetWS, constraintToWorld );
constraint.axes[0].SetAxisFriction( -2, 2, 20 );
constraint.axes[1].SetAxisFriction( 0, 0, 0 );
constraint.axes[2].SetAxisFriction( -15, 15, 20 );
// Exaggerate the bone's ability to pull the mass of the ragdoll around
constraint.constraint.bodyMassScale[1] = 50.0f;
pAttached->GetPositionMatrix( &tmp );
MatrixInvert( tmp, worldToAttached );
pRefObject->GetPositionMatrix( &tmp );
MatrixInvert( tmp, worldToReference );
ConcatTransforms( worldToReference, constraintToWorld, constraint.constraintToReference );
ConcatTransforms( worldToAttached, constraintToWorld, constraint.constraintToAttached );
// for now, just slam this to be the passed in value
MatrixSetColumn( attachmentPointRagdollSpace, 3, constraint.constraintToReference );
PhysDisableEntityCollisions( pAttached, m_ragdoll.list[0].pObject );
m_pAttachConstraint = physenv->CreateRagdollConstraint( pRefObject, pAttached, m_ragdoll.pGroup, constraint );
SetParent( pFollow );
SetOwnerEntity( pFollow );
RagdollActivate( m_ragdoll, modelinfo->GetVCollide( GetModelIndex() ), GetModelIndex() );
// add a bunch of dampening to the ragdoll
for ( int i = 0; i < m_ragdoll.listCount; i++ )
{
float damping, rotdamping;
m_ragdoll.list[i].pObject->GetDamping( &damping, &rotdamping );
damping *= ATTACHED_DAMPING_SCALE;
rotdamping *= ATTACHED_DAMPING_SCALE;
m_ragdoll.list[i].pObject->SetDamping( &damping, &rotdamping );
}
m_boneIndexAttached = boneIndexRoot;
m_ragdollAttachedObjectIndex = ragdollAttachedIndex;
m_attachmentPointBoneSpace = boneLocalOrigin;
Vector vTemp;
MatrixGetColumn( constraint.constraintToReference, 3, vTemp );
m_attachmentPointRagdollSpace = vTemp;
}
void CWeaponIFMSteadyCam::UpdateRelativeOrientation()
{
if ( m_bIsLocked )
return;
if ( m_bInDirectMode )
{
UpdateDirectRelativeOrientation();
return;
}
if ( ( m_vecViewOffset.x == 0.0f ) && ( m_vecViewOffset.y == 0.0f ) )
return;
// Compute a player to steadycam matrix
VMatrix steadyCamToPlayer;
MatrixFromAngles( m_angRelativeAngles, steadyCamToPlayer );
MatrixSetColumn( steadyCamToPlayer, 3, m_vecRelativePosition );
Vector vecCurrentForward;
MatrixGetColumn( steadyCamToPlayer, 0, &vecCurrentForward );
// Create a ray in steadycam space
float flMaxD = 1.0f / tan( M_PI * m_flFOV / 360.0f );
// Remap offsets into normalized space
float flViewX = m_vecViewOffset.x / ( 384 / 2 );
float flViewY = m_vecViewOffset.y / ( 288 / 2 );
flViewX *= flMaxD * ifm_steadycam_mousefactor.GetFloat();
flViewY *= flMaxD * ifm_steadycam_mousefactor.GetFloat();
Vector vecSelectionDir( 1.0f, -flViewX, -flViewY );
VectorNormalize( vecSelectionDir );
// Rotate the ray into player coordinates
Vector vecDesiredDirection;
Vector3DMultiply( steadyCamToPlayer, vecSelectionDir, vecDesiredDirection );
float flDot = DotProduct( vecDesiredDirection, vecCurrentForward );
flDot = clamp( flDot, -1.0f, 1.0f );
float flAngle = 180.0f * acos( flDot ) / M_PI;
if ( flAngle < 1e-3 )
{
matrix3x4_t mat;
MatrixFromForwardDirection( vecDesiredDirection, mat );
MatrixAngles( mat, m_angRelativeAngles );
return;
}
Vector vecAxis;
CrossProduct( vecCurrentForward, vecDesiredDirection, vecAxis );
VectorNormalize( vecAxis );
float flRotateRate = ifm_steadycam_rotaterate.GetFloat();
if ( flRotateRate < 1.0f )
{
flRotateRate = 1.0f;
}
float flRateFactor = flAngle / flRotateRate;
flRateFactor *= flRateFactor * flRateFactor;
float flRate = flRateFactor * 30.0f;
float flMaxAngle = gpGlobals->frametime * flRate;
flAngle = clamp( flAngle, 0.0f, flMaxAngle );
Vector vecNewForard;
VMatrix rotation;
MatrixBuildRotationAboutAxis( rotation, vecAxis, flAngle );
Vector3DMultiply( rotation, vecCurrentForward, vecNewForard );
matrix3x4_t mat;
MatrixFromForwardDirection( vecNewForard, mat );
MatrixAngles( mat, m_angRelativeAngles );
Assert( m_angRelativeAngles.IsValid() );
}
void CRagdollProp::InitRagdoll( const Vector &forceVector, int forceBone, const Vector &forcePos, matrix3x4_t *pPrevBones, matrix3x4_t *pBoneToWorld, float dt, int collisionGroup, bool activateRagdoll )
{
SetCollisionGroup( collisionGroup );
// Make sure it's interactive debris for at most 5 seconds
if ( collisionGroup == COLLISION_GROUP_INTERACTIVE_DEBRIS )
{
SetContextThink( &CRagdollProp::SetDebrisThink, gpGlobals->curtime + 5, s_pDebrisContext );
}
SetMoveType( MOVETYPE_VPHYSICS );
SetSolid( SOLID_VPHYSICS );
AddSolidFlags( FSOLID_CUSTOMRAYTEST | FSOLID_CUSTOMBOXTEST );
m_takedamage = DAMAGE_EVENTS_ONLY;
ragdollparams_t params;
params.pGameData = static_cast<void *>( static_cast<CBaseEntity *>(this) );
params.modelIndex = GetModelIndex();
params.pCollide = modelinfo->GetVCollide( params.modelIndex );
params.pStudioHdr = GetModelPtr();
params.forceVector = forceVector;
params.forceBoneIndex = forceBone;
params.forcePosition = forcePos;
params.pPrevBones = pPrevBones;
params.pCurrentBones = pBoneToWorld;
params.boneDt = dt;
params.jointFrictionScale = 1.0;
RagdollCreate( m_ragdoll, params, physenv );
if ( m_anglesOverrideString != NULL_STRING && Q_strlen(m_anglesOverrideString.ToCStr()) > 0 )
{
char szToken[2048];
const char *pStr = nexttoken(szToken, STRING(m_anglesOverrideString), ',');
// anglesOverride is index,angles,index,angles (e.g. "1, 22.5 123.0 0.0, 2, 0 0 0, 3, 0 0 180.0")
while ( szToken[0] != 0 )
{
int objectIndex = atoi(szToken);
// sanity check to make sure this token is an integer
Assert( atof(szToken) == ((float)objectIndex) );
pStr = nexttoken(szToken, pStr, ',');
Assert( szToken[0] );
if ( objectIndex >= m_ragdoll.listCount )
{
Warning("Bad ragdoll pose in entity %s, model (%s) at %s, model changed?\n", GetDebugName(), GetModelName().ToCStr(), VecToString(GetAbsOrigin()) );
}
else if ( szToken[0] != 0 )
{
QAngle angles;
Assert( objectIndex >= 0 && objectIndex < RAGDOLL_MAX_ELEMENTS );
UTIL_StringToVector( angles.Base(), szToken );
int boneIndex = m_ragdoll.boneIndex[objectIndex];
AngleMatrix( angles, pBoneToWorld[boneIndex] );
const ragdollelement_t &element = m_ragdoll.list[objectIndex];
Vector out;
if ( element.parentIndex >= 0 )
{
int parentBoneIndex = m_ragdoll.boneIndex[element.parentIndex];
VectorTransform( element.originParentSpace, pBoneToWorld[parentBoneIndex], out );
}
else
{
out = GetAbsOrigin();
}
MatrixSetColumn( out, 3, pBoneToWorld[boneIndex] );
element.pObject->SetPositionMatrix( pBoneToWorld[boneIndex], true );
}
pStr = nexttoken(szToken, pStr, ',');
}
}
if ( activateRagdoll )
{
MEM_ALLOC_CREDIT();
RagdollActivate( m_ragdoll, params.pCollide, GetModelIndex() );
}
for ( int i = 0; i < m_ragdoll.listCount; i++ )
{
UpdateNetworkDataFromVPhysics( m_ragdoll.list[i].pObject, i );
g_pPhysSaveRestoreManager->AssociateModel( m_ragdoll.list[i].pObject, GetModelIndex() );
physcollision->CollideGetAABB( m_ragdollMins[i], m_ragdollMaxs[i], m_ragdoll.list[i].pObject->GetCollide(), vec3_origin, vec3_angle );
}
VPhysicsSetObject( m_ragdoll.list[0].pObject );
CalcRagdollSize();
}
//-----------------------------------------------------------------------------
// Set up the bones for a frame
//-----------------------------------------------------------------------------
matrix3x4_t* CIHVTestApp::SetUpBones( studiohdr_t *pStudioHdr, const matrix3x4_t &shapeToWorld, int iRun, int model, int boneMask )
{
// Default to middle of the pose parameter range
float pPoseParameter[MAXSTUDIOPOSEPARAM];
for ( int i = 0; i < MAXSTUDIOPOSEPARAM; ++i )
{
pPoseParameter[i] = 0.5f;
}
CStudioHdr studioHdr( pStudioHdr, g_pMDLCache );
int nFrameCount = Studio_MaxFrame( &studioHdr, g_BenchRuns[iRun].sequence1[model], pPoseParameter );
if ( nFrameCount == 0 )
{
nFrameCount = 1;
}
Vector pos[MAXSTUDIOBONES];
Quaternion q[MAXSTUDIOBONES];
InitPose( &studioHdr, pos, q, boneMask );
AccumulatePose( &studioHdr, NULL, pos, q, g_BenchRuns[iRun].sequence1[model], s_Cycle[model], pPoseParameter, boneMask, 1.0f, 0.0 );
// FIXME: Try enabling this?
// CalcAutoplaySequences( pStudioHdr, NULL, pos, q, pPoseParameter, BoneMask( ), flTime );
// Root transform
matrix3x4_t rootToWorld, temp;
MatrixCopy( shapeToWorld, rootToWorld );
matrix3x4_t *pBoneToWorld = g_pStudioRender->LockBoneMatrices( studioHdr.numbones() );
for ( int i = 0; i < studioHdr.numbones(); i++ )
{
// If it's not being used, fill with NAN for errors
if ( !(studioHdr.pBone( i )->flags & boneMask) )
{
int j, k;
for (j = 0; j < 3; j++)
{
for (k = 0; k < 4; k++)
{
pBoneToWorld[i][j][k] = VEC_T_NAN;
}
}
continue;
}
matrix3x4_t boneMatrix;
QuaternionMatrix( q[i], boneMatrix );
MatrixSetColumn( pos[i], 3, boneMatrix );
if (studioHdr.pBone(i)->parent == -1)
{
ConcatTransforms (rootToWorld, boneMatrix, pBoneToWorld[i]);
}
else
{
ConcatTransforms (pBoneToWorld[ studioHdr.pBone(i)->parent ], boneMatrix, pBoneToWorld[i] );
}
}
g_pStudioRender->UnlockBoneMatrices();
return pBoneToWorld;
}
static int vmatrix_MatrixSetColumn (lua_State *L) {
MatrixSetColumn(luaL_checkvmatrix(L, 1), luaL_checkint(L, 2), luaL_checkvector(L, 3));
return 0;
}
bool _ComputeRagdollBones( const ragdoll_t *pRagdoll, matrix3x4_t &parentTransform, matrix3x4_t *pBones, Vector *pPositions, QAngle *pAngles )
{
matrix3x4_t inverted, output;
#ifdef _DEBUG
CBitVec<MAXSTUDIOBONES> vBonesComputed;
vBonesComputed.ClearAll();
#endif
for ( int i = 0; i < pRagdoll->listCount; ++i )
{
const ragdollelement_t& element = pRagdoll->list[ i ];
// during restore if a model has changed since the file was saved, this could be NULL
if ( !element.pObject )
return false;
int const boneIndex = pRagdoll->boneIndex[ i ];
if ( boneIndex < 0 )
{
AssertMsg( 0, "Replay: No mapping for ragdoll bone\n" );
return false;
}
// Get global transform and put it into the bone cache
element.pObject->GetPositionMatrix( &pBones[ boneIndex ] );
// Ensure a fixed translation from the parent (no stretching)
if ( element.parentIndex >= 0 && !pRagdoll->allowStretch )
{
int parentIndex = pRagdoll->boneIndex[ element.parentIndex ];
#ifdef _DEBUG
// Make sure we computed the parent already
Assert( vBonesComputed.IsBitSet(parentIndex) );
#endif
// overwrite the position from physics to force rigid attachment
// NOTE: On the client we actually override this with the proper parent bone in each LOD
Vector out;
VectorTransform( element.originParentSpace, pBones[ parentIndex ], out );
MatrixSetColumn( out, 3, pBones[ boneIndex ] );
MatrixInvert( pBones[ parentIndex ], inverted );
}
else if ( element.parentIndex == - 1 )
{
// Decompose into parent space
MatrixInvert( parentTransform, inverted );
}
#ifdef _DEBUG
vBonesComputed.Set( boneIndex, true );
#endif
// Compute local transform and put into 'output'
ConcatTransforms( inverted, pBones[ boneIndex ], output );
// Cache as Euler/position
MatrixAngles( output, pAngles[ i ], pPositions[ i ] );
}
return true;
}
void CDmeMDL::SetUpBones( CStudioHdr &studioHdr, const matrix3x4_t& shapeToWorld, int nMaxBoneCount, matrix3x4_t *pBoneToWorld )
{
// Default to middle of the pose parameter range
float pPoseParameter[MAXSTUDIOPOSEPARAM];
for ( int i = 0; i < MAXSTUDIOPOSEPARAM; ++i )
{
pPoseParameter[i] = 0.5f;
}
int nFrameCount = Studio_MaxFrame( &studioHdr, m_nSequence, pPoseParameter );
if ( nFrameCount == 0 )
{
nFrameCount = 1;
}
float flCycle = ( m_flTime * m_flPlaybackRate ) / nFrameCount;
// FIXME: We're always wrapping; may want to determing if we should clamp
flCycle -= (int)(flCycle);
Vector pos[MAXSTUDIOBONES];
Quaternion q[MAXSTUDIOBONES];
InitPose( &studioHdr, pos, q, BoneMask( ) );
AccumulatePose( &studioHdr, NULL, pos, q, m_nSequence, flCycle, pPoseParameter, BoneMask( ), 1.0f, m_flTime );
// FIXME: Try enabling this?
// CalcAutoplaySequences( pStudioHdr, NULL, pos, q, pPoseParameter, BoneMask( ), flTime );
// Root transform
matrix3x4_t rootToWorld, temp;
// Rotate the root transform to make it align with DMEs
// DMEs up vector is the y axis
if ( !m_bDrawInEngine )
{
matrix3x4_t engineToDme;
EngineToDmeMatrix( engineToDme );
ConcatTransforms( engineToDme, shapeToWorld, rootToWorld );
}
else
{
MatrixCopy( shapeToWorld, rootToWorld );
}
if ( nMaxBoneCount > studioHdr.numbones() )
{
nMaxBoneCount = studioHdr.numbones();
}
for ( int i = 0; i < nMaxBoneCount; i++ )
{
// If it's not being used, fill with NAN for errors
#ifdef _DEBUG
if ( !(studioHdr.pBone( i )->flags & BoneMask()))
{
int j, k;
for (j = 0; j < 3; j++)
{
for (k = 0; k < 4; k++)
{
pBoneToWorld[i][j][k] = VEC_T_NAN;
}
}
continue;
}
#endif
matrix3x4_t boneMatrix;
QuaternionMatrix( q[i], boneMatrix );
MatrixSetColumn( pos[i], 3, boneMatrix );
if (studioHdr.pBone(i)->parent == -1)
{
ConcatTransforms( rootToWorld, boneMatrix, pBoneToWorld[i] );
}
else
{
ConcatTransforms( pBoneToWorld[ studioHdr.pBone(i)->parent ], boneMatrix, pBoneToWorld[i] );
}
}
}