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 ); } }