void SBVAR::SetupSBVAR(void)
{
TData_predetermined *data=(TData_predetermined*)pData();
YY=lambda*TransposeMultiply(data->Data(),data->Data());
XX=lambda*TransposeMultiply(data->PredeterminedData(),data->PredeterminedData());
XY=lambda*TransposeMultiply(data->PredeterminedData(),data->Data());
log_likelihood_constant=-lambda_T*n_vars*0.918938533204673; // 0.918938533204673 = 0.5*ln(2*pi)
delete data;
}
/*
================
idPhysics_RigidBody::CheckForCollisions
Check for collisions between the current and next state.
If there is a collision the next state is set to the state at the moment of impact.
================
*/
bool idPhysics_RigidBody::CheckForCollisions( const float deltaTime, rigidBodyPState_t &next, trace_t &collision ) {
//#define TEST_COLLISION_DETECTION
idMat3 axis;
idRotation rotation;
bool collided = false;
#ifdef TEST_COLLISION_DETECTION
bool startsolid;
if( gameLocal.clip.Contents( current.i.position, clipModel, current.i.orientation, clipMask, self ) ) {
startsolid = true;
}
#endif
TransposeMultiply( current.i.orientation, next.i.orientation, axis );
rotation = axis.ToRotation();
rotation.SetOrigin( current.i.position );
// if there was a collision
if( gameLocal.clip.Motion( collision, current.i.position, next.i.position, rotation, clipModel, current.i.orientation, clipMask, self ) ) {
// set the next state to the state at the moment of impact
next.i.position = collision.endpos;
next.i.orientation = collision.endAxis;
next.i.linearMomentum = current.i.linearMomentum;
next.i.angularMomentum = current.i.angularMomentum;
collided = true;
}
#ifdef TEST_COLLISION_DETECTION
if( gameLocal.clip.Contents( next.i.position, clipModel, next.i.orientation, clipMask, self ) ) {
if( !startsolid ) {
int bah = 1;
}
}
#endif
return collided;
}
bool CAR_DKW_o::SetParameters_InitializeABOmega()
{
if (!CAR_DKW::SetParameters_InitializeABOmega())
return false;
TDenseMatrix lambda1 = SIGMA_inverse * SIGMAlambda1;
TDenseVector KAPPAtheta = KAPPA * theta;
TDenseVector theta_Q;
aI_Q.Zeros(dataP->MATgrid_options.Dimension());
bI_Q.Zeros(dataP->MATgrid_options.Dimension(), Nfac);
for (int i=0; i<dataP->MATgrid_options.Dimension(); i++)
{
double MAT = dataP->MATgrid_options(i);
TDenseVector temp_ay;
TDenseMatrix temp_by;
if (!YieldFacLoad(temp_ay, temp_by, KAPPA_rn, Inv_KAPPA_rn, Inv_Kron_KAPPA_rn, SIGMA, KAPPAtheta, rho0, rho1, lambda0,TDenseVector(1,MAT)))
return false;
TDenseVector temp_by_vector = temp_by.RowVector(0); // -MAT * temp_by.RowVector(0);
theta_Q = Multiply(Inv_KAPPA_rn, KAPPAtheta-SIGMA*lambda0+MultiplyTranspose(SIGMA,SIGMA)*temp_by_vector);
double rho0_Q = rho0_pi - InnerProduct(lambda0, sigq)+InnerProduct(sigq, TransposeMultiply(SIGMA,temp_by_vector));
TDenseVector rho1_Q = rho1_pi - TransposeMultiply(lambda1, sigq);
double temp_aI_Q;
TDenseVector temp_bI_Q;
InfExpFacLoad(temp_aI_Q, temp_bI_Q, KAPPA_rn, Inv_KAPPA_rn, Inv_Kron_KAPPA_rn, SIGMA, theta_Q, sigq, sigqx, rho0_Q, rho1_Q, MAT);
aI_Q(i) = temp_aI_Q;
bI_Q.InsertRowMatrix(i, 0, temp_bI_Q);
}
return true;
}
/*
See Waggoner and Zha, "A Gibbs sampler for structural vector autoregressions",
JEDC 2003, for discription of notations. We take the square root of a
symmetric and positive definite X to be any matrix Y such that Y*Y'=X. Note
that this is not the usual definition because we do not require Y to be
symmetric and positive definite.
*/
void SBVAR_symmetric_linear::SetPriorSimulationInfo(void)
{
if (flat_prior)
throw dw_exception("flat prior not allowed if simulating from prior");
PriorSimulate_SqrtVariance.resize(n_vars);
TDenseMatrix X;
for (int i=n_vars-1; i >= 0; i--)
{
TDenseMatrix S(dim_b[i]+dim_g[i],dim_b[i]+dim_g[i]);
S.Insert(0,0,TransposeMultiply(U[i],prior_YY*U[i]));
S.Insert(dim_b[i],0,X=-TransposeMultiply(V[i],prior_XY*U[i]));
S.Insert(0,dim_b[i],Transpose(X));
S.Insert(dim_b[i],dim_b[i],TransposeMultiply(V[i],prior_XX*V[i]));
PriorSimulate_SqrtVariance[i]=Inverse(Cholesky(S,CHOLESKY_UPPER_TRIANGULAR),SOLVE_UPPER_TRIANGULAR);
}
prior_simulation_info_set=true;
}
TDenseMatrix ConditionalVariance(const TDenseMatrix &A0)
{
try
{
return Inverse(TransposeMultiply(A0,A0));
}
catch (dw_exception &e)
{
throw dw_exception("Reduced Form(): A0 is singular");
}
}
void SBVAR_symmetric::SetupSBVAR_symmetric(void)
{
prior_YY=TransposeMultiply(prior_Y,prior_Y);
prior_XX=TransposeMultiply(prior_X,prior_X);
prior_XY=TransposeMultiply(prior_X,prior_Y);
if (flat_prior)
log_prior_constant=0.0;
else
{
TDenseMatrix S(n_vars+n_predetermined,n_vars+n_predetermined);
S.Insert(0,0,prior_YY);
S.Insert(n_vars,0,-prior_XY);
S.Insert(0,n_vars,-Transpose(prior_XY));
S.Insert(n_vars,n_vars,prior_XX);
log_prior_constant=n_vars*(-0.918938533204673*(n_vars+n_predetermined) + 0.5*LogAbsDeterminant(S)); // 0.918938533204673 = 0.5*ln(2*pi)
}
prior_YY*=lambda_bar;
prior_XX*=lambda_bar;
prior_XY*=lambda_bar;
log_prior_constant*=lambda_bar;
}
// Assumes that there is either no exogenous variables or there is a single
// exogenous variable that is constant and equal to one. The unconditional
// mean is obtained from the reduced form companion matrix.
TDenseMatrix UnconditionalVariance(const TDenseMatrix &A0, const TDenseMatrix &Aplus, bool IsConstant)
{
int n_lags=NumberLags(A0,Aplus,IsConstant), n_vars=A0.cols;
TDenseMatrix B=ReducedForm(A0,Aplus);
if (n_lags == 0) return ConditionalVariance(A0);
TDenseMatrix C=CompanionMatrix(B,n_lags), V=BlockDiagonalMatrix(A0,n_lags),
X=V*(Identity(n_vars*n_lags) - C);
try
{
return SubMatrix(Inverse(TransposeMultiply(X,X)),0,n_vars-1,0,n_vars-1);
}
catch (dw_exception &e)
{
throw dw_exception("UnconditionalMean(): Unconditional mean does not exist");
}
}
void CMeleeWeapon::CheckAttack( idVec3 OldOrigin, idMat3 OldAxis )
{
trace_t tr;
idMat3 axis;
idClipModel *pClip;
int ClipMask;
if( m_WeapClip )
pClip = m_WeapClip;
else
pClip = GetPhysics()->GetClipModel();
idVec3 NewOrigin = GetPhysics()->GetOrigin() + m_ClipOffset;
idMat3 NewAxis = GetPhysics()->GetAxis() * m_ClipRotation;
TransposeMultiply( OldAxis, NewAxis, axis );
idRotation rotation = axis.ToRotation();
rotation.SetOrigin( OldOrigin );
// before collision test, check for AI in range whose head CMs we should modify
// for best accuracy, we should run this every frame here in CheckAttack
// this accounts for enemy and AI closing at high velocities
// If this causes slowdown, we can comment it out and lose the accuracy
if( m_bModAICMs )
CheckAICMSwaps();
if( m_bWorldCollide )
ClipMask = CONTENTS_MELEE_WORLDCOLLIDE;
else
ClipMask = CONTENTS_MELEE_ACTCOLLIDE; // parries and AI
// TODO: Is CONTENTS_BODY going to hit the outer collision box and cause problems?
// Hack: We really want to ignore more than one entity, but we can't do that,
// so temporarily set our CONTENTS so that we don't hit ourself
int contentsEnt = GetPhysics()->GetContents();
GetPhysics()->SetContents( CONTENTS_FLASHLIGHT_TRIGGER );
// Also ignore the owner for this trace
int contentsOwner = m_Owner.GetEntity()->GetPhysics()->GetContents();
m_Owner.GetEntity()->GetPhysics()->SetContents( CONTENTS_FLASHLIGHT_TRIGGER );
// Uncomment for debugging
/*
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING
(
"Check Attack called, old origin %s, new origin %s, old axis %s, new axis %s\r",
OldOrigin.ToString(), NewOrigin.ToString(), OldAxis.ToString(), NewAxis.ToString()
);
*/
if( cv_melee_debug.GetBool() )
collisionModelManager->DrawModel( pClip->Handle(), NewOrigin, NewAxis, gameLocal.GetLocalPlayer()->GetEyePosition(), idMath::INFINITY );
gameLocal.clip.Motion
(
tr, OldOrigin, NewOrigin,
rotation, pClip, OldAxis,
ClipMask, m_Owner.GetEntity()
);
GetPhysics()->SetContents( contentsEnt );
m_Owner.GetEntity()->GetPhysics()->SetContents( contentsOwner );
// ishtvan: new approach calling separate function HandleValidCollision:
if( tr.fraction < 1.0f )
{
// hit something
// Calculate the instantaneous velocity _direction_ of the point that hit
// For now, we just need direction of velocity, not magnitude, don't need delta_t
idVec3 vLinearTrans = (NewOrigin - OldOrigin);
// Calc. translation due to angular velocity
// velocity of point = r x w
// Keep in mind that rotation was defined as a rotation about origin,
// not about the center of mass
idVec3 r = tr.c.point - OldOrigin;
idVec3 vSpinTrans = r.Cross((rotation.ToAngularVelocity()));
idVec3 PointVelDir = vLinearTrans + vSpinTrans;
// TODO: Divide by delta_t if we want to store velocity magnitude later
PointVelDir.Normalize();
HandleValidCollision( tr, OldOrigin, PointVelDir );
}
// ishtvan: Old approach, moved this into another function
// leave the commented code around for a while until we're sure we didn't break something
/*
// hit something
if( tr.fraction < 1.0f )
{
idEntity *other = gameLocal.entities[ tr.c.entityNum ];
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: Hit entity %s\r", other->name.c_str());
// Show the initial trace collision point
if( cv_melee_debug.GetBool() )
gameRenderWorld->DebugArrow( colorBlue,OldOrigin, tr.c.point, 3, 1000 );
location = JOINT_HANDLE_TO_CLIPMODEL_ID( tr.c.id );
// Calculate the instantaneous velocity _direction_ of the point that hit
// For now, we just need direction of velocity, not magnitude, don't need delta_t
idVec3 vLinearTrans = (NewOrigin - OldOrigin);
// Calc. translation due to angular velocity
// velocity of point = r x w
// Keep in mind that rotation was defined as a rotation about origin,
// not about the center of mass
idVec3 r = tr.c.point - OldOrigin;
idVec3 vSpinTrans = r.Cross((rotation.ToAngularVelocity()));
idVec3 PointVelDir = vLinearTrans + vSpinTrans;
// TODO: Divide by delta_t if we want to store velocity magnitude later
PointVelDir.Normalize();
// Secondary trace for when we hit the AF structure of an AI and want
// to see where we would hit on the actual model
if( (tr.c.contents & CONTENTS_CORPSE)
&& other->IsType(idAFEntity_Base::Type) )
{
idAFEntity_Base *otherAF = static_cast<idAFEntity_Base *>(other);
trace_t tr2;
// NOTE: Just extrapolating along the velocity can fail when the AF
// extends outside of the rendermodel
// Just using the AF face normal can fail when hit at a corner
// So take an equal average of both
idVec3 trDir = ( PointVelDir - tr.c.normal ) / 2.0f;
trDir.Normalize();
idVec3 start = tr.c.point - 8.0f * trDir;
contentsEnt = GetPhysics()->GetContents();
GetPhysics()->SetContents( CONTENTS_FLASHLIGHT_TRIGGER );
gameLocal.clip.TracePoint
(
tr2, start, tr.c.point + 8.0f * trDir,
CONTENTS_RENDERMODEL, m_Owner.GetEntity()
);
GetPhysics()->SetContents( contentsEnt );
// Ishtvan: Ignore secondary trace if we hit a swapped-in head model on the first pass
// we want to register a hit on that swapped in head body for gameplay reasons
bool bHitSwappedHead = false;
if( otherAF->IsType(idAI::Type) )
{
idAI *otherAI = static_cast<idAI *>(otherAF);
if( otherAI->m_bHeadCMSwapped && (tr.c.id == otherAI->m_HeadBodyID) )
bHitSwappedHead = true;
}
if( tr2.fraction < 1.0f && !bHitSwappedHead )
{
tr = tr2;
other = gameLocal.entities[ tr.c.entityNum ];
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: CONTENTS_CORPSE secondary trace hit entity %s\r", other->name.c_str());
// Draw the new collision point
if( cv_melee_debug.GetBool() )
{
gameRenderWorld->DebugArrow( colorRed, start, tr2.c.point, 3, 1000 );
}
other = gameLocal.entities[tr2.c.entityNum];
// update location
location = JOINT_HANDLE_TO_CLIPMODEL_ID( tr.c.id );
}
else
{
// failed to connect with the rendermodel.
// Last ditch effort to find the correct AF body
location = otherAF->JointForBody(tr.c.id);
// If we failed to find anything, draw the attempted trace in green
if( cv_melee_debug.GetBool() )
gameRenderWorld->DebugArrow( colorGreen, start, tr.c.point + 8.0f * PointVelDir, 3, 1000 );
}
// Uncomment for translational and angular velocity debugging
if( cv_melee_debug.GetBool() )
{
idVec3 vLinDir = vLinearTrans;
idVec3 vSpinDir = vSpinTrans;
vLinDir.Normalize();
vSpinDir.Normalize();
gameRenderWorld->DebugArrow
(
colorCyan, (tr.c.point - 8.0f * vLinDir),
(tr.c.point + 8.0f * vLinDir), 3, 1000
);
// Uncomment for translational and angular velocity debugging
gameRenderWorld->DebugArrow
(
colorPurple, (tr.c.point - 8.0f * vSpinDir),
(tr.c.point + 8.0f * vSpinDir), 3, 1000
);
}
}
// Direction of the velocity of point that hit (renamed it for brevity)
idVec3 dir = PointVelDir;
idActor *AttachOwner(NULL);
idEntity *OthBindMaster(NULL);
if( other->IsType(idAFAttachment::Type) )
{
idEntity *othBody = static_cast<idAFAttachment *>(other)->GetBody();
if( othBody->IsType(idActor::Type) )
AttachOwner = static_cast<idActor *>(othBody);
}
// Also check for any object bound to an actor (helmets, etc)
else if( (OthBindMaster = other->GetBindMaster()) != NULL
&& OthBindMaster->IsType(idActor::Type) )
AttachOwner = static_cast<idActor *>(OthBindMaster);
CMeleeStatus *pStatus = &m_Owner.GetEntity()->m_MeleeStatus;
// Check if we hit a melee parry or held object
// (for some reason tr.c.contents erroneously returns CONTENTS_MELEEWEAP for everything except the world)
// if( (tr.c.contents & CONTENTS_MELEEWEAP) != 0 )
// this doesn't always work either, it misses linked clipmodels on melee weapons
// if( other->GetPhysics() && ( other->GetPhysics()->GetContents(tr.c.id) & CONTENTS_MELEEWEAP) )
// have to do this to catch all cases:
if
(
(other->GetPhysics() && ( other->GetPhysics()->GetContents(tr.c.id) & CONTENTS_MELEEWEAP))
|| ( other->IsType(CMeleeWeapon::Type) && static_cast<CMeleeWeapon *>(other)->GetOwner() )
)
{
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: Hit someting with CONTENTS_MELEEWEAP\r");
// hit a parry (make sure we don't hit our own other melee weapons)
if( other->IsType(CMeleeWeapon::Type)
&& static_cast<CMeleeWeapon *>(other)->GetOwner()
&& static_cast<CMeleeWeapon *>(other)->GetOwner() != m_Owner.GetEntity() )
{
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING
("MeleeWeapon: Hit a melee parry put up by %s\r",
static_cast<CMeleeWeapon *>(other)->GetOwner()->name.c_str() );
// Test our attack against their parry
TestParry( static_cast<CMeleeWeapon *>(other), dir, &tr );
}
// hit a held object
else if( other == gameLocal.m_Grabber->GetSelected() )
{
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: Hit an object held by the player\r");
MeleeCollision( other, dir, &tr, location );
// TODO: Message the grabber that the grabbed object has been hit
// So that it can fly out of player's hands if desired
// update AI status (consider this a miss)
pStatus->m_ActionResult = MELEERESULT_AT_MISSED;
pStatus->m_ActionPhase = MELEEPHASE_RECOVERING;
// TODO: Message the attacking AI to play a bounce off animation if appropriate
DeactivateAttack();
}
else
{
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: Hit something with CONTENTS_MELEEWEAP that's not an active parry or a held object (this shouldn't normally happen).\r");
MeleeCollision( other, dir, &tr, location );
// update AI status (consider this a miss)
pStatus->m_ActionResult = MELEERESULT_AT_MISSED;
pStatus->m_ActionPhase = MELEEPHASE_RECOVERING;
DeactivateAttack();
}
}
// Hit an actor, or an AF attachment that is part of an actor
else if( other->IsType(idActor::Type) || AttachOwner != NULL )
{
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: Hit actor or part of actor %s\r", other->name.c_str());
idActor *owner = m_Owner.GetEntity();
idActor *otherAct;
if( AttachOwner )
otherAct = AttachOwner;
else
otherAct = static_cast<idActor *>(other);
// Don't do anything if we hit our own AF attachment
// AI also don't do damage to friendlies/neutral
if( AttachOwner != owner
&& !(owner->IsType(idAI::Type) && !(static_cast<idAI *>(owner)->IsEnemy(otherAct))) )
{
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: Hit AI other than ourselves.\r");
// TODO: Scale damage with instantaneous velocity of the blade?
MeleeCollision( other, dir, &tr, location );
// update actor's melee status
pStatus->m_ActionResult = MELEERESULT_AT_HIT;
pStatus->m_ActionPhase = MELEEPHASE_RECOVERING;
DeactivateAttack();
}
else
{
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: Hit yourself. Stop hitting yourself!\r");
}
}
// Hit something else in the world (only happens to the player)
else
{
DM_LOG(LC_WEAPON,LT_DEBUG)LOGSTRING("MeleeWeapon: Hit a non-AI object: %s\r", other->name.c_str());
MeleeCollision( other, dir, &tr, location );
// TODO: Message the attacking actor to play a bounce off animation if appropriate
// keep the attack going if the object hit is a moveable below a certain mass
// TODO: Handle moveables bound to other things and use the total mass of the system?
if( !( other->IsType(idMoveable::Type) && other->GetPhysics()->GetMass() < m_StopMass ) )
{
// message the AI, consider this a miss
pStatus->m_ActionResult = MELEERESULT_AT_MISSED;
pStatus->m_ActionPhase = MELEEPHASE_RECOVERING;
DeactivateAttack();
}
}
}
*/
}
/*
================
sdScriptedEntityHelper_Aimer::SetTarget
================
*/
void sdScriptedEntityHelper_Aimer::SetTarget( const idVec3& _target ) {
const idMat3& bindAxes = _owner->GetRenderEntity()->axis;
const idVec3& bindOrg = _owner->GetRenderEntity()->origin;
idMat3 yawJointAxis;
idVec3 yawJointOrg;
if ( gunJoints[ AIMER_JOINT_SHOULDER ] != INVALID_JOINT ) {
idMat3 currentAxes;
idVec3 currentOrigin;
_owner->GetAnimator()->GetJointTransform( gunJoints[ AIMER_JOINT_SHOULDER ], gameLocal.time, currentOrigin, currentAxes );
idMat3 transform;
TransposeMultiply( baseAxes[ AIMER_JOINT_SHOULDER ], currentAxes, transform );
idMat3 shoulderAxis = bindAxes;
idVec3 shoulderOrg = bindOrg + ( currentOrigin * shoulderAxis );
yawJointAxis = transform * shoulderAxis;
yawJointOrg = shoulderOrg + ( yawJointAxis * ikPaths[ AIMER_IK_BASE ] );
} else {
yawJointAxis = bindAxes;
yawJointOrg = bindOrg + ( yawJointAxis * ikPaths[ AIMER_IK_BASE ] );
}
{
idVec3 target = _target;
target -= yawJointOrg;
target *= yawJointAxis.Transpose();
target.z = 0.f;
yawInfo.ideal = RAD2DEG( atan2( target.y, target.x ) );
float angle;
if ( CalcAngle( target.Length(), yawInfo.arcLength, yawInfo.offsetAngle, angle ) ) {
yawInfo.ideal += angle - yawInfo.initialAngle;
}
}
if ( yawInfo.clamp.flags.enabled ) {
yawInfo.ideal = idMath::ClampFloat( yawInfo.clamp.extents[ 0 ], yawInfo.clamp.extents[ 1 ], yawInfo.ideal );
}
idMat3 yawAxes;
idAngles::YawToMat3( yawInfo.ideal, yawAxes );
idMat3 pitchJointAxis = yawAxes * yawJointAxis;
idVec3 pitchJointOrg = yawJointOrg + ( pitchJointAxis * ikPaths[ AIMER_IK_YAW ] );
{
idVec3 target = _target;
target -= pitchJointOrg;
target *= pitchJointAxis.Transpose();
target.y = 0.f;
pitchInfo.ideal = RAD2DEG( atan2( target.z, target.x ) );
float angle;
if ( CalcAngle( target.Length(), pitchInfo.arcLength, pitchInfo.offsetAngle, angle ) ) {
pitchInfo.ideal += angle - pitchInfo.initialAngle;
}
pitchInfo.ideal = -pitchInfo.ideal; // pitch is inverted for some reason...
}
if ( pitchInfo.clamp.flags.enabled ) {
pitchInfo.ideal = idMath::ClampFloat( pitchInfo.clamp.extents[ 0 ], pitchInfo.clamp.extents[ 1 ], pitchInfo.ideal );
}
}
/*
================
sdVehicleIKArms::Update
================
*/
void sdVehicleIKArms::Update( void ) {
idEntity* vehicleEnt = vehicle;
if ( !vehicleEnt ) {
return;
}
idVec3 shoulderPos;
idMat3 temp;
idAnimator* animator = vehicleEnt->GetAnimator();
animator->GetJointTransform( ikJoints[ ARM_JOINT_INDEX_SHOULDER ], gameLocal.time, shoulderPos, temp );
idVec3 shoulderToElbow = baseJointPositions[ ARM_JOINT_INDEX_ELBOW ] - baseJointPositions[ ARM_JOINT_INDEX_SHOULDER ];
idVec3 elbowToWrist = baseJointPositions[ ARM_JOINT_INDEX_WRIST ] - baseJointPositions[ ARM_JOINT_INDEX_ELBOW ];
idVec3 wristToMuzzle = baseJointPositions[ ARM_JOINT_INDEX_MUZZLE ] - baseJointPositions[ ARM_JOINT_INDEX_WRIST ];
idVec3 elbowToMuzzle = baseJointPositions[ ARM_JOINT_INDEX_MUZZLE ] - baseJointPositions[ ARM_JOINT_INDEX_ELBOW ];
idMat3 shoulderAxis;
TransposeMultiply( baseJointAxes[ ARM_JOINT_INDEX_SHOULDER ], temp, shoulderAxis );
idMat3 transposedShoulderAxis = shoulderAxis.Transpose();
idPlayer* player = GetPlayer();
if ( player && requireTophat ) {
if ( !gameLocal.usercmds[ player->entityNumber ].buttons.btn.tophat ) {
player = NULL;
}
}
bool changed = false;
changed |= !oldParentAxis.Compare( temp, 0.005f );
idAngles newAngles;
renderView_t* view = player ? player->GetRenderView() : NULL;
renderEntity_t* renderEnt = vehicleEnt->GetRenderEntity();
trace_t trace;
idVec3 modelTarget;
if( view ) {
idVec3 end = view->vieworg + ( 8192 * view->viewaxis[ 0 ] );
gameLocal.clip.TracePoint( CLIP_DEBUG_PARMS trace, view->vieworg, end, CONTENTS_SOLID | CONTENTS_OPAQUE, player );
modelTarget = trace.endpos;
modelTarget -= renderEnt->origin;
modelTarget *= renderEnt->axis.Transpose();
modelTarget -= shoulderPos;
modelTarget *= transposedShoulderAxis;
modelTarget -= shoulderToElbow;
}
if ( view ) {
idVec3 target = modelTarget;
const idVec3& dir = baseJointAxes[ ARM_JOINT_INDEX_MUZZLE ][ 0 ];
target -= elbowToMuzzle - ( ( dir * elbowToMuzzle ) * dir );
target *= baseJointAxes[ ARM_JOINT_INDEX_MUZZLE ].Transpose();
newAngles.yaw = RAD2DEG( atan2( target[ 1 ], target[ 0 ] ) );
} else {
newAngles.yaw = 0;
}
bool yawChanged = !sdVehiclePosition::ClampAngle( newAngles, jointAngles, clampYaw, 1, 0.1f );
if ( yawSound != NULL ) {
yawSound->Update( yawChanged );
}
changed |= yawChanged;
idMat3 yawMat;
idAngles::YawToMat3( newAngles.yaw, yawMat );
if ( view ) {
idVec3 target = modelTarget;
idVec3 newElbowToWrist = elbowToWrist * yawMat;
idVec3 newWristToMuzzle = wristToMuzzle * yawMat;
idMat3 muzzleAxis = baseJointAxes[ ARM_JOINT_INDEX_MUZZLE ] * yawMat;
target -= newElbowToWrist;
target -= newWristToMuzzle - ( ( muzzleAxis[ 0 ] * newWristToMuzzle ) * muzzleAxis[ 0 ] );
target *= muzzleAxis.Transpose();
newAngles.pitch = -RAD2DEG( atan2( target[ 2 ], target[ 0 ] ) );
} else {
newAngles.pitch = 0;
}
bool pitchChanged = !sdVehiclePosition::ClampAngle( newAngles, jointAngles, clampPitch, 0, 0.1f );
if ( pitchSound != NULL ) {
pitchSound->Update( pitchChanged );
}
changed |= pitchChanged;
// configurable pitching axis - to support vertically oriented arms (eg badger, bumblebee)
// as well as horizontally oriented arms (eg goliath)
int truePitchAxis = pitchAxis;
if ( truePitchAxis < 0 ) {
newAngles.pitch = -newAngles.pitch;
truePitchAxis = -truePitchAxis;
}
idAngles pitchAngles( 0.0f, 0.0f, 0.0f );
if ( truePitchAxis == 1 ) { // x-axis
pitchAngles.roll = newAngles.pitch;
} else if ( truePitchAxis == 3 ) { // z-axis
pitchAngles.yaw = newAngles.pitch;
} else { // y-axis
pitchAngles.pitch = newAngles.pitch;
}
idMat3 pitchMat = pitchAngles.ToMat3();
if( changed ) {
oldParentAxis = temp;
jointAngles = newAngles;
animator->SetJointAxis( ikJoints[ ARM_JOINT_INDEX_ELBOW ], JOINTMOD_WORLD_OVERRIDE, baseJointAxes[ ARM_JOINT_INDEX_ELBOW ] * yawMat * shoulderAxis );
animator->SetJointAxis( ikJoints[ ARM_JOINT_INDEX_WRIST ], JOINTMOD_WORLD_OVERRIDE, pitchMat * baseJointAxes[ ARM_JOINT_INDEX_WRIST ] * yawMat * shoulderAxis );
}
}
