const bool Test_Matrix_RotationMatrix()
{
const TVector3d kAxis_d = Normalize(TVector3d(), TVector3d{1.0, 1.0, 1.0});
const double kdAngle = s_kdTau / 8.0;
const TVector4d kQuatOrient_d = AxisAngleQuaternion(TVector4d(), kAxis_d, kdAngle);
const TMatrix4d kOrientation_d = RotationMatrix(TMatrix4d(), kQuatOrient_d);
const TVector4d kTransformedA_d = VectorMultiply(TVector4d(), kOrientation_d, TVector4d{1.0, 0.0, 0.0, 1.0});
const TVector3d kTransformedB_d{kTransformedA_d.m_dX, kTransformedA_d.m_dY, kTransformedA_d.m_dZ};
const TVector3d kTransformedC_d = QuaternionRotate(TVector3d(), TVector3d{1.0, 0.0, 0.0}, kQuatOrient_d);
const bool kbPass_d = Equal(kTransformedB_d, kTransformedC_d, s_kdEpsilon);
const TVector3f kAxis_f = Normalize(TVector3f(), TVector3f{1.0f, 1.0f, 1.0f});
const float kfAngle = s_kfTau / 8.0f;
const TVector4f kQuatOrient_f = AxisAngleQuaternion(TVector4f(), kAxis_f, kfAngle);
const TMatrix4f kOrientation_f = RotationMatrix(TMatrix4f(), kQuatOrient_f);
const TVector4f kTransformedA_f = VectorMultiply(TVector4f(), kOrientation_f, TVector4f{1.0f, 0.0f, 0.0f, 1.0f});
const TVector3f kTransformedB_f{kTransformedA_f.m_fX, kTransformedA_f.m_fY, kTransformedA_f.m_fZ};
const TVector3f kTransformedC_f = QuaternionRotate(TVector3f(), TVector3f{1.0f, 0.0f, 0.0f}, kQuatOrient_f);
const bool kbPass_f = Equal(kTransformedB_f, kTransformedC_f, s_kfEpsilon);
return(kbPass_d && kbPass_f);
}
const bool Test_Matrix_TransformationMatrix()
{
const TVector3d kTranslate_d{1.0, 2.0, 3.0};
const TVector3d kAxis_d = Normalize(TVector3d(), TVector3d{1.0, 1.0, 1.0});
const double kdAngle = s_kdTau / 8.0;
const TVector4d kQuatOrient_d = AxisAngleQuaternion(TVector4d(), kAxis_d, kdAngle);
const double kdScale = 2.0;
const TMatrix4d kTranslation_d = TranslationMatrix(TMatrix4d(), kTranslate_d);
const TMatrix4d kOrientation_d = RotationMatrix(TMatrix4d(), kQuatOrient_d);
const TMatrix4d kScaling_d = ScalingMatrix(TMatrix4d(), kdScale, kdScale, kdScale);
const TMatrix4d kTransformA_d = Multiply(TMatrix4d(), kScaling_d, kOrientation_d);
const TMatrix4d kTransformB_d = Multiply(TMatrix4d(), kTranslation_d, kTransformA_d);
const TVector3d kBasisX_d = QuaternionRotate(TVector3d(), TVector3d{kdScale, 0.0, 0.0}, kQuatOrient_d);
const TVector3d kBasisY_d = QuaternionRotate(TVector3d(), TVector3d{0.0, kdScale, 0.0}, kQuatOrient_d);
const TVector3d kBasisZ_d = QuaternionRotate(TVector3d(), TVector3d{0.0, 0.0, kdScale}, kQuatOrient_d);
const TMatrix4d kTransformC_d = TransformationMatrix(TMatrix4d(), kBasisX_d, kBasisY_d, kBasisZ_d, kTranslate_d);
const bool kbPass_d = Equal(kTransformC_d, kTransformB_d, s_kdEpsilon);
//
const TVector3f kTranslate_f{1.0f, 2.0f, 3.0f};
const TVector3f kAxis_f = Normalize(TVector3f(), TVector3f{1.0f, 1.0f, 1.0f});
const float kfAngle = s_kfTau / 8.0f;
const TVector4f kQuatOrient_f = AxisAngleQuaternion(TVector4f(), kAxis_f, kfAngle);
const float kfScale = 2.0f;
const TMatrix4f kTranslation_f = TranslationMatrix(TMatrix4f(), kTranslate_f);
const TMatrix4f kOrientation_f = RotationMatrix(TMatrix4f(), kQuatOrient_f);
const TMatrix4f kScaling_f = ScalingMatrix(TMatrix4f(), kfScale, kfScale, kfScale);
const TMatrix4f kTransformA_f = Multiply(TMatrix4f(), kScaling_f, kOrientation_f);
const TMatrix4f kTransformB_f = Multiply(TMatrix4f(), kTranslation_f, kTransformA_f);
const TVector3f kBasisX_f = QuaternionRotate(TVector3f(), TVector3f{kfScale, 0.0f, 0.0f}, kQuatOrient_f);
const TVector3f kBasisY_f = QuaternionRotate(TVector3f(), TVector3f{0.0f, kfScale, 0.0f}, kQuatOrient_f);
const TVector3f kBasisZ_f = QuaternionRotate(TVector3f(), TVector3f{0.0f, 0.0f, kfScale}, kQuatOrient_f);
const TMatrix4f kTransformC_f = TransformationMatrix(TMatrix4f(), kBasisX_f, kBasisY_f, kBasisZ_f, kTranslate_f);
const bool kbPass_f = Equal(kTransformC_f, kTransformB_f, s_kfEpsilon);
return(kbPass_d && kbPass_f);
}
void CAI_Spotlight::UpdateSpotlightDirection( void )
{
if ( !m_hSpotlight )
{
CreateSpotlightEntities();
}
// Compute the current beam direction
Vector vTargetDir;
VectorSubtract( m_vSpotlightTargetPos, m_hSpotlight->GetAbsStartPos(), vTargetDir );
VectorNormalize(vTargetDir);
ConstrainToCone( &vTargetDir );
// Compute the amount to rotate
float flDot = DotProduct( vTargetDir, m_vSpotlightDir );
flDot = clamp( flDot, -1.0f, 1.0f );
float flAngle = AngleNormalize( RAD2DEG( acos( flDot ) ) );
float flClampedAngle = clamp( flAngle, 0.0f, 45.0f );
float flBeamTurnRate = SimpleSplineRemapVal( flClampedAngle, 0.0f, 45.0f, 10.0f, 45.0f );
if ( fabs(flAngle) > flBeamTurnRate * gpGlobals->frametime )
{
flAngle = flBeamTurnRate * gpGlobals->frametime;
}
// Compute the rotation axis
Vector vecRotationAxis;
CrossProduct( m_vSpotlightDir, vTargetDir, vecRotationAxis );
if ( VectorNormalize( vecRotationAxis ) < 1e-3 )
{
vecRotationAxis.Init( 0, 0, 1 );
}
// Compute the actual rotation amount, using quat slerp blending
Quaternion desiredQuat, resultQuat;
AxisAngleQuaternion( vecRotationAxis, flAngle, desiredQuat );
QuaternionSlerp( m_vAngularVelocity, desiredQuat, QUAT_BLEND_FACTOR, resultQuat );
m_vAngularVelocity = resultQuat;
// If we're really close, and we're not moving very quickly, slam.
float flActualRotation = AngleNormalize( RAD2DEG(2 * acos(m_vAngularVelocity.w)) );
if (( flActualRotation < 1e-3 ) && (flAngle < 1e-3 ))
{
m_vSpotlightDir = vTargetDir;
m_vAngularVelocity.Init( 0, 0, 0, 1 );
return;
}
// Update the desired direction
matrix3x4_t rot;
Vector vecNewDir;
QuaternionMatrix( m_vAngularVelocity, rot );
VectorRotate( m_vSpotlightDir, rot, vecNewDir );
m_vSpotlightDir = vecNewDir;
VectorNormalize(m_vSpotlightDir);
}
SOPAngle *SOPAngle::RotateAroundAxis(SOPVector *vec, lua_Number degrees) {
matrix3x4_t m_rgflCoordinateFrame;
Vector rotationAxisLs;
Quaternion q;
matrix3x4_t xform;
matrix3x4_t localToWorldMatrix;
Vector axisvector = vec->ToVector();
QAngle rotatedAngles;
QAngle angOurAngs = ToQAngle();
AngleMatrix( angOurAngs, m_rgflCoordinateFrame );
VectorIRotate( axisvector, m_rgflCoordinateFrame, rotationAxisLs );
AxisAngleQuaternion( rotationAxisLs, degrees, q );
QuaternionMatrix( q, vec3_origin, xform );
ConcatTransforms( m_rgflCoordinateFrame, xform, localToWorldMatrix );
MatrixAngles( localToWorldMatrix, rotatedAngles );
return new SOPAngle(rotatedAngles.x, rotatedAngles.y, rotatedAngles.z);
}
//------------------------------------------------------------------------------
// Constrain to cone
//------------------------------------------------------------------------------
bool CAI_Spotlight::ConstrainToCone( Vector *pDirection )
{
Vector vecOrigin, vecForward;
if ( m_nSpotlightAttachment == 0 )
{
QAngle vecAngles;
vecAngles = GetOuter()->GetAbsAngles();
AngleVectors( vecAngles, &vecForward );
}
else
{
GetOuter()->GetAttachment( m_nSpotlightAttachment, vecOrigin, &vecForward );
}
if ( m_flConstraintAngle == 0.0f )
{
*pDirection = vecForward;
return true;
}
float flDot = DotProduct( vecForward, *pDirection );
if ( flDot >= cos( DEG2RAD( m_flConstraintAngle ) ) )
return false;
Vector vecAxis;
CrossProduct( *pDirection, vecForward, vecAxis );
VectorNormalize( vecAxis );
Quaternion q;
AxisAngleQuaternion( vecAxis, -m_flConstraintAngle, q );
Vector vecResult;
matrix3x4_t rot;
QuaternionMatrix( q, rot );
VectorRotate( vecForward, rot, vecResult );
VectorNormalize( vecResult );
*pDirection = vecResult;
return true;
}
const bool Test_Matrix_AxisRotationMatrix()
{
const TVector3d kAxisXd{1.0, 0.0, 0.0};
const TVector3d kAxisYd{0.0, 1.0, 0.0};
const TVector3d kAxisZd{0.0, 0.0, 1.0};
const double kdAngle = s_kdTau / 8.0;
const TVector4d kQRotationXd = AxisAngleQuaternion(TVector4d(), kAxisXd, kdAngle);
const TVector4d kQRotationYd = AxisAngleQuaternion(TVector4d(), kAxisYd, kdAngle);
const TVector4d kQRotationZd = AxisAngleQuaternion(TVector4d(), kAxisZd, kdAngle);
const bool kbPassXd = Equal(AxisRotationXMatrix(TMatrix4d(), -kdAngle), RotationMatrix(TMatrix4d(), kQRotationXd), s_kdEpsilon);
const bool kbPassYd = Equal(AxisRotationYMatrix(TMatrix4d(), -kdAngle), RotationMatrix(TMatrix4d(), kQRotationYd), s_kdEpsilon);
const bool kbPassZd = Equal(AxisRotationZMatrix(TMatrix4d(), -kdAngle), RotationMatrix(TMatrix4d(), kQRotationZd), s_kdEpsilon);
const TVector3f kAxisXf{1.0f, 0.0f, 0.0f};
const TVector3f kAxisYf{0.0f, 1.0f, 0.0f};
const TVector3f kAxisZf{0.0f, 0.0f, 1.0f};
const float kfAngle = s_kfTau / 8.0f;
const TVector4f kQRotationXf = AxisAngleQuaternion(TVector4f(), kAxisXf, -kfAngle);
const TVector4f kQRotationYf = AxisAngleQuaternion(TVector4f(), kAxisYf, -kfAngle);
const TVector4f kQRotationZf = AxisAngleQuaternion(TVector4f(), kAxisZf, -kfAngle);
const bool kbPassXf = Equal(AxisRotationXMatrix(TMatrix4f(), kfAngle), RotationMatrix(TMatrix4f(), kQRotationXf), s_kfEpsilon);
const bool kbPassYf = Equal(AxisRotationYMatrix(TMatrix4f(), kfAngle), RotationMatrix(TMatrix4f(), kQRotationYf), s_kfEpsilon);
const bool kbPassZf = Equal(AxisRotationZMatrix(TMatrix4f(), kfAngle), RotationMatrix(TMatrix4f(), kQRotationZf), s_kfEpsilon);
return( kbPassXd
&& kbPassYd
&& kbPassZd
&& kbPassXf
&& kbPassYf
&& kbPassZf);
}
