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