float RadiusFromBounds (const vec3_t mins, const vec3_t maxs) { vec3_t m1, m2; VectorMultiply(mins, mins, m1); VectorMultiply(maxs, maxs, m2); return sqrt(max(m1[0], m2[0]) + max(m1[1], m2[1]) + max(m1[2], m2[2])); }
float RadiusFromBoundsAndOrigin (const vec3_t mins, const vec3_t maxs, const vec3_t origin) { vec3_t m1, m2; VectorSubtract(mins, origin, m1);VectorMultiply(m1, m1, m1); VectorSubtract(maxs, origin, m2);VectorMultiply(m2, m2, m2); return sqrt(max(m1[0], m2[0]) + max(m1[1], m2[1]) + max(m1[2], m2[2])); }
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); }
static FORCEINLINE bool IntersectBoxWithPermutedPlanes( const FConvexVolume::FPermutedPlaneArray& PermutedPlanes, const VectorRegister BoxOrigin, const VectorRegister BoxExtent ) { bool Result = true; checkSlow(PermutedPlanes.Num() % 4 == 0); // Splat origin into 3 vectors VectorRegister OrigX = VectorReplicate(BoxOrigin, 0); VectorRegister OrigY = VectorReplicate(BoxOrigin, 1); VectorRegister OrigZ = VectorReplicate(BoxOrigin, 2); // Splat extent into 3 vectors VectorRegister ExtentX = VectorReplicate(BoxExtent, 0); VectorRegister ExtentY = VectorReplicate(BoxExtent, 1); VectorRegister ExtentZ = VectorReplicate(BoxExtent, 2); // Splat the abs for the pushout calculation VectorRegister AbsExt = VectorAbs(BoxExtent); VectorRegister AbsExtentX = VectorReplicate(AbsExt, 0); VectorRegister AbsExtentY = VectorReplicate(AbsExt, 1); VectorRegister AbsExtentZ = VectorReplicate(AbsExt, 2); // Since we are moving straight through get a pointer to the data const FPlane* RESTRICT PermutedPlanePtr = (FPlane*)PermutedPlanes.GetData(); // Process four planes at a time until we have < 4 left for ( int32 Count = 0, Num = PermutedPlanes.Num(); Count < Num; Count += 4 ) { // Load 4 planes that are already all Xs, Ys, ... VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; // Calculate the distance (x * x) + (y * y) + (z * z) - w VectorRegister DistX = VectorMultiply(OrigX,PlanesX); VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX); VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY); VectorRegister Distance = VectorSubtract(DistZ,PlanesW); // Now do the push out FMath::Abs(x * x) + FMath::Abs(y * y) + FMath::Abs(z * z) VectorRegister PushX = VectorMultiply(AbsExtentX,VectorAbs(PlanesX)); VectorRegister PushY = VectorMultiplyAdd(AbsExtentY,VectorAbs(PlanesY),PushX); VectorRegister PushOut = VectorMultiplyAdd(AbsExtentZ,VectorAbs(PlanesZ),PushY); // Check for completely outside if ( VectorAnyGreaterThan(Distance,PushOut) ) { Result = false; break; } } return Result; }
const bool Test_Matrix_ScalingMatrix() { const TMatrix4d kScaleMatrix_d = ScalingMatrix(TMatrix4d(), 1.0, 2.0, 3.0); const TVector4d kTransformedPoint_d = VectorMultiply(TVector4d(), kScaleMatrix_d, TVector4d{1.0, 1.0, 1.0, 1.0}); const bool kbPass_d = Equal(kTransformedPoint_d, TVector4d{1.0, 2.0, 3.0, 1.0}, s_kdEpsilon); const TMatrix4f kScaleMatrix_f = ScalingMatrix(TMatrix4f(), 1.0f, 2.0f, 3.0f); const TVector4f kTransformedPoint_f = VectorMultiply(TVector4f(), kScaleMatrix_f, TVector4f{1.0f, 1.0f, 1.0f, 1.0f}); const bool kbPass_f = Equal(kTransformedPoint_f, TVector4f{1.0f, 2.0f, 3.0f, 1.0f}, s_kfEpsilon); return(kbPass_d && kbPass_f); }
void Luce(double x, double y, double z,double dimensione,float r,float g,float b,float iox,float ioy,float ioz,int Tex) { Vector Position; Vector MyPosition; Position.x = x; Position.y = y; Position.z = z; MyPosition.x=iox; MyPosition.y=ioy; MyPosition.z=ioz; Vector sight = VectorDiff(MyPosition, Position); Vector cz; cz.x = 0; cz.y = 0; cz.z = 1; Vector cross1 = VectorMultiply( sight, cz ); Vector cross2 = VectorMultiply( sight, cross1 ); cross1 = VectorNormalize(cross1); cross2 = VectorNormalize(cross2); cross1 = VectorScalarMultiply(cross1, dimensione); cross2 = VectorScalarMultiply(cross2, dimensione); glColor3f(r,g,b); glEnable(GL_TEXTURE_2D); glEnable (GL_BLEND); glBlendFunc( (1,1,1,1), (1,1,1,1)); glDepthMask (GL_FALSE); glBindTexture( GL_TEXTURE_2D, texture[Tex] ); glBegin(GL_QUADS); glTexCoord2d( 0.0, 0.0 ); glVertex3d( Position.x + cross1.x, Position.y + cross1.y, Position.z + cross1.z); glTexCoord2d( 1.0, 0.0 ); glVertex3d( Position.x - cross2.x, Position.y - cross2.y, Position.z - cross2.z); glTexCoord2d( 1.0, 1.0 ); glVertex3d( Position.x - cross1.x, Position.y - cross1.y, Position.z - cross1.z); glTexCoord2d( 0.0, 1.0 ); glVertex3d( Position.x + cross2.x, Position.y + cross2.y, Position.z + cross2.z); glEnd(); glDisable(GL_TEXTURE_2D); glDisable (GL_BLEND); glDepthMask (GL_TRUE); }
//----------------------------------------------------------------------------- // Compute world center of a prop //----------------------------------------------------------------------------- static void ComputeWorldCenter( DetailObjectLump_t& prop, Vector& center, Vector& normal ) { // Transform the offset into world space Vector forward, right; AngleVectors( prop.m_Angles, &forward, &right, &normal ); VectorCopy( prop.m_Origin, center ); // FIXME: Take orientation into account? switch (prop.m_Type ) { case DETAIL_PROP_TYPE_MODEL: VectorMA( center, g_ModelCenterOffset[prop.m_DetailModel].x, forward, center ); VectorMA( center, -g_ModelCenterOffset[prop.m_DetailModel].y, right, center ); VectorMA( center, g_ModelCenterOffset[prop.m_DetailModel].z, normal, center ); break; case DETAIL_PROP_TYPE_SPRITE: Vector vecOffset; VectorMultiply( g_SpriteCenterOffset[prop.m_DetailModel], prop.m_flScale, vecOffset ); VectorMA( center, vecOffset.x, forward, center ); VectorMA( center, -vecOffset.y, right, center ); VectorMA( center, vecOffset.z, normal, center ); break; } }
//----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- void CPropAPC::FireRocket( void ) { if ( m_flRocketTime > gpGlobals->curtime ) return; // If we're still firing the salvo, fire quickly m_iRocketSalvoLeft--; if ( m_iRocketSalvoLeft > 0 ) { m_flRocketTime = gpGlobals->curtime + ROCKET_DELAY_TIME; } else { // Reload the salvo m_iRocketSalvoLeft = ROCKET_SALVO_SIZE; m_flRocketTime = gpGlobals->curtime + random->RandomFloat( ROCKET_MIN_BURST_PAUSE_TIME, ROCKET_MAX_BURST_PAUSE_TIME ); } Vector vecRocketOrigin; GetRocketShootPosition( &vecRocketOrigin ); static float s_pSide[] = { 0.966, 0.866, 0.5, -0.5, -0.866, -0.966 }; Vector forward; GetVectors( &forward, NULL, NULL ); Vector vecDir; CrossProduct( Vector( 0, 0, 1 ), forward, vecDir ); vecDir.z = 1.0f; vecDir.x *= s_pSide[m_nRocketSide]; vecDir.y *= s_pSide[m_nRocketSide]; if ( ++m_nRocketSide >= 6 ) { m_nRocketSide = 0; } VectorNormalize( vecDir ); Vector vecVelocity; VectorMultiply( vecDir, ROCKET_SPEED, vecVelocity ); QAngle angles; VectorAngles( vecDir, angles ); CAPCMissile *pRocket = (CAPCMissile *)CAPCMissile::Create( vecRocketOrigin, angles, vecVelocity, this ); pRocket->IgniteDelay(); if ( m_hSpecificRocketTarget ) { pRocket->AimAtSpecificTarget( m_hSpecificRocketTarget ); m_hSpecificRocketTarget = NULL; } else if ( m_strMissileHint != NULL_STRING ) { pRocket->SetGuidanceHint( STRING( m_strMissileHint ) ); } EmitSound( "PropAPC.FireRocket" ); m_OnFiredMissile.FireOutput( this, this ); }
//----------------------------------------------------------------------------- //----------------------------------------------------------------------------- void CMapData_Server::GetMapOrigin( Vector &vecOrigin ) { Vector vecMins, vecMaxs; GetMapBounds( vecMins, vecMaxs ); VectorAdd( vecMins, vecMaxs, vecOrigin ); VectorMultiply( vecOrigin, 0.5f, vecOrigin ); }
const bool Test_Matrix_TranslationMatrix() { const TVector3d kTranslate_d{1.0, 2.0, 3.0}; const TMatrix4d kTranslationMatrix_d = TranslationMatrix(TMatrix4d(), kTranslate_d); const TVector4d kTranslatedPointA_d = VectorMultiply(TVector4d(), kTranslationMatrix_d, TVector4d{0.0, 0.0, 0.0, 1.0}); const TVector3d kTranslatedPointB_d{kTranslatedPointA_d.m_dX, kTranslatedPointA_d.m_dY, kTranslatedPointA_d.m_dZ}; const bool kbPass_d = Equal(kTranslate_d, kTranslatedPointB_d, s_kdEpsilon); const TVector3f kTranslate_f{1.0f, 2.0f, 3.0f}; const TMatrix4f kTranslationMatrix_f = TranslationMatrix(TMatrix4f(), kTranslate_f); const TVector4f kTranslatedPointA_f = VectorMultiply(TVector4f(), kTranslationMatrix_f, TVector4f{0.0f, 0.0f, 0.0f, 1.0f}); const TVector3f kTranslatedPointB_f{kTranslatedPointA_f.m_fX, kTranslatedPointA_f.m_fY, kTranslatedPointA_f.m_fZ}; const bool kbPass_f = Equal(kTranslate_f, kTranslatedPointB_f, s_kfEpsilon); return(kbPass_d && kbPass_f); }
void CClientLeafSystem::ProjectShadow( ClientLeafShadowHandle_t handle, const Vector& origin, const Vector& dir, const Vector2D& size, float maxDist ) { // Remove the shadow from any leaves it current exists in RemoveShadowFromLeaves( handle ); RemoveShadowFromRenderables( handle ); Assert( ( m_Shadows[handle].m_Flags & SHADOW_FLAGS_PROJECTED_TEXTURE_TYPE_MASK ) == SHADOW_FLAGS_SHADOW ); // This will help us to avoid adding the shadow multiple times to a renderable ++m_ShadowEnum; // Create a ray starting at the origin, with a boxsize == to the // maximum size, and cast it along the direction of the shadow // Then mark each leaf that the ray hits with the shadow Ray_t ray; VectorCopy( origin, ray.m_Start ); VectorMultiply( dir, maxDist, ray.m_Delta ); ray.m_StartOffset.Init( 0, 0, 0 ); float maxsize = max( size.x, size.y ) * 0.5f; ray.m_Extents.Init( maxsize, maxsize, maxsize ); ray.m_IsRay = false; ray.m_IsSwept = true; CShadowLeafEnum leafEnum; ISpatialQuery* pQuery = engine->GetBSPTreeQuery(); pQuery->EnumerateLeavesAlongRay( ray, &leafEnum, handle ); }
void CMultiplyProxy::OnBind( void *pC_BaseEntity ) { Assert( m_pSrc1 && m_pSrc2 && m_pResult ); MaterialVarType_t resultType; int vecSize; ComputeResultType( resultType, vecSize ); switch( resultType ) { case MATERIAL_VAR_TYPE_VECTOR: { Vector a, b, c; m_pSrc1->GetVecValue( a.Base(), vecSize ); m_pSrc2->GetVecValue( b.Base(), vecSize ); VectorMultiply( a, b, c ); m_pResult->SetVecValue( c.Base(), vecSize ); } break; case MATERIAL_VAR_TYPE_FLOAT: SetFloatResult( m_pSrc1->GetFloatValue() * m_pSrc2->GetFloatValue() ); break; case MATERIAL_VAR_TYPE_INT: m_pResult->SetFloatValue( m_pSrc1->GetIntValue() * m_pSrc2->GetIntValue() ); break; } if ( ToolsEnabled() ) { ToolFramework_RecordMaterialParams( GetMaterial() ); } }
//----------------------------------------------------------------------------- // Compute ambient lighting component at specified position. //----------------------------------------------------------------------------- static void ComputeAmbientLightingAtPoint( int iThread, const Vector &origin, Vector radcolor[NUMVERTEXNORMALS], Vector color[MAX_LIGHTSTYLES] ) { // NOTE: I'm not dealing with shadow-casting static props here // This is for speed, although we can add it if it turns out to // be important // sample world by casting N rays distributed across a sphere Vector upend; int j; for ( j = 0; j < MAX_LIGHTSTYLES; ++j) { color[j].Init( 0,0,0 ); } float tanTheta = tan(VERTEXNORMAL_CONE_INNER_ANGLE); for (int i = 0; i < NUMVERTEXNORMALS; i++) { VectorMA( origin, COORD_EXTENT * 1.74, g_anorms[i], upend ); // Now that we've got a ray, see what surface we've hit CalcRayAmbientLighting( iThread, origin, upend, tanTheta, color ); // DumpRayToGlView( ray, surfEnum.m_HitFrac, &color[0], "test.out" ); } for ( j = 0; j < MAX_LIGHTSTYLES; ++j) { VectorMultiply( color[j], 255.0f / (float)NUMVERTEXNORMALS, color[j] ); } }
FTransform FTransform::GetRelativeTransform(const FTransform& Other) const { // A * B(-1) = VQS(B)(-1) (VQS (A)) // // Scale = S(A)/S(B) // Rotation = Q(B)(-1) * Q(A) // Translation = 1/S(B) *[Q(B)(-1)*(T(A)-T(B))*Q(B)] // where A = this, B = Other FTransform Result; if (Other.IsRotationNormalized() == false) { return FTransform::Identity; } // Scale = S(A)/S(B) static ScalarRegister STolerance(SMALL_NUMBER); VectorRegister VSafeScale3D = VectorSet_W0(GetSafeScaleReciprocal(Other.Scale3D, STolerance)); VectorRegister VScale3D = VectorMultiply(Scale3D, VSafeScale3D); //VQTranslation = ( ( T(A).X - T(B).X ), ( T(A).Y - T(B).Y ), ( T(A).Z - T(B).Z), 0.f ); VectorRegister VQTranslation = VectorSet_W0(VectorSubtract(Translation, Other.Translation)); // Translation = 1/S(B) *[Q(B)(-1)*(T(A)-T(B))*Q(B)] VectorRegister VInverseRot = VectorQuaternionInverse(Other.Rotation); VectorRegister VQT = VectorQuaternionMultiply2(VInverseRot, VQTranslation); VectorRegister VR = VectorQuaternionMultiply2(VQT, Other.Rotation); VectorRegister VTranslation = VectorMultiply(VR, VSafeScale3D); // Rotation = Q(B)(-1) * Q(A) VectorRegister VRotation = VectorQuaternionMultiply2(VInverseRot, Rotation ); Result.Scale3D = VScale3D; Result.Translation = VTranslation; Result.Rotation = VRotation; Result.DiagnosticCheckNaN_All(); #if DEBUG_INVERSE_TRANSFORM FMatrix AM = ToMatrixWithScale(); FMatrix BM = Other.ToMatrixWithScale(); Result.DebugEqualMatrix(AM * BM.InverseFast()); #endif return Result; }
//----------------------------------------------------------------------------- // Computes the position of the canister //----------------------------------------------------------------------------- void CEnvHeadcrabCanisterShared::GetPositionAtTime( float flTime, Vector &vecPosition, QAngle &vecAngles ) { float flDeltaTime = flTime - m_flLaunchTime; if ( flDeltaTime > m_flFlightTime ) { flDeltaTime = m_flFlightTime; } VMatrix initToWorld; if ( m_bLaunchedFromWithinWorld || m_bInSkybox ) { VectorMA( m_vecStartPosition, flDeltaTime * m_flHorizSpeed, m_vecParabolaDirection, vecPosition ); vecPosition.z += m_flInitialZSpeed * flDeltaTime + 0.5f * m_flZAcceleration * flDeltaTime * flDeltaTime; Vector vecLeft; CrossProduct( m_vecParabolaDirection, Vector( 0, 0, 1 ), vecLeft ); Vector vecForward; VectorMultiply( m_vecParabolaDirection, -1.0f, vecForward ); vecForward.z = -(m_flInitialZSpeed + m_flZAcceleration * flDeltaTime) / m_flHorizSpeed; // This is -dz/dx. VectorNormalize( vecForward ); Vector vecUp; CrossProduct( vecForward, vecLeft, vecUp ); initToWorld.SetBasisVectors( vecForward, vecLeft, vecUp ); } else { flDeltaTime -= m_flWorldEnterTime; Vector vecVelocity; VectorMultiply( m_vecDirection, m_flFlightSpeed, vecVelocity ); VectorMA( m_vecEnterWorldPosition, flDeltaTime, vecVelocity, vecPosition ); MatrixFromAngles( m_vecStartAngles.Get(), initToWorld ); } VMatrix rotation; MatrixBuildRotationAboutAxis( rotation, Vector( 1, 0, 0 ), flDeltaTime * ROTATION_SPEED ); VMatrix newAngles; MatrixMultiply( initToWorld, rotation, newAngles ); MatrixToAngles( newAngles, vecAngles ); }
//----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- void CHarpoon::HarpoonTouch( CBaseEntity *pOther ) { // If we've stuck something, freeze. Make sure we hit it along our velocity. if ( pOther->GetCollisionGroup() != TFCOLLISION_GROUP_SHIELD ) { // Perform the collision response... const trace_t &tr = CBaseEntity::GetTouchTrace( ); Vector vecNewVelocity; PhysicsClipVelocity (GetAbsVelocity(), tr.plane.normal, vecNewVelocity, 2.0 - GetFriction()); SetAbsVelocity( vecNewVelocity ); } else { // Move away from the shield... // Fling it out a little extra along the plane normal Vector vecCenter; AngleVectors( pOther->GetAbsAngles(), &vecCenter ); Vector vecNewVelocity; VectorMultiply( vecCenter, 400.0f, vecNewVelocity ); SetAbsVelocity( vecNewVelocity ); } if ( !pOther->IsBSPModel() && !pOther->GetBaseAnimating() ) return; // At this point, it shouldn't affect player movement SetCollisionGroup( COLLISION_GROUP_DEBRIS ); // Remove myself soon SetThink( SUB_Remove ); SetNextThink( gpGlobals->curtime + 30.0 ); m_hImpaledTarget = pOther; // Should I impale something? if ( pOther->GetBaseAnimating() ) { CheckLinkedHarpoon(); if ( pOther->GetMoveType() != MOVETYPE_NONE ) { ImpaleTarget( pOther ); return; } } CheckLinkedHarpoon(); EmitSound( "Harpoon.Impact" ); // Stop moving SetMoveType( MOVETYPE_NONE ); }
bool FConvexVolume::IntersectSphere(const FVector& Origin,const float& Radius, bool& bOutFullyContained) const { bool Result = true; //Assume fully contained bOutFullyContained = true; checkSlow(PermutedPlanes.Num() % 4 == 0); // Load the origin & radius VectorRegister Orig = VectorLoadFloat3(&Origin); VectorRegister VRadius = VectorLoadFloat1(&Radius); VectorRegister NegativeVRadius = VectorNegate(VRadius); // Splat origin into 3 vectors VectorRegister OrigX = VectorReplicate(Orig, 0); VectorRegister OrigY = VectorReplicate(Orig, 1); VectorRegister OrigZ = VectorReplicate(Orig, 2); // Since we are moving straight through get a pointer to the data const FPlane* RESTRICT PermutedPlanePtr = (FPlane*)PermutedPlanes.GetData(); // Process four planes at a time until we have < 4 left for (int32 Count = 0; Count < PermutedPlanes.Num(); Count += 4) { // Load 4 planes that are already all Xs, Ys, ... VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; // Calculate the distance (x * x) + (y * y) + (z * z) - w VectorRegister DistX = VectorMultiply(OrigX,PlanesX); VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX); VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY); VectorRegister Distance = VectorSubtract(DistZ,PlanesW); // Check for completely outside int32 Mask = VectorAnyGreaterThan(Distance,VRadius); if (Mask) { Result = false; bOutFullyContained = false; break; } //the sphere is definitely inside the frustums, but let's check if it's FULLY contained by checking the NEGATIVE radius (on the inside of each frustum plane) Mask = VectorAnyGreaterThan(Distance,NegativeVRadius); if (Mask) { bOutFullyContained = false; } } return Result; }
FTransform FTransform::GetRelativeTransformReverse(const FTransform& Other) const { // A (-1) * B = VQS(B)(VQS (A)(-1)) // // Scale = S(B)/S(A) // Rotation = Q(B) * Q(A)(-1) // Translation = T(B)-S(B)/S(A) *[Q(B)*Q(A)(-1)*T(A)*Q(A)*Q(B)(-1)] // where A = this, and B = Other FTransform Result; // Scale = S(B)/S(A) VectorRegister VSafeScale3D = VectorSet_W0(GetSafeScaleReciprocal(Scale3D)); VectorRegister VScale3D = VectorMultiply(Other.Scale3D, VSafeScale3D); // Rotation = Q(B) * Q(A)(-1) VectorRegister VInverseRot = VectorQuaternionInverse(Rotation); VectorRegister VRotation = VectorQuaternionMultiply2(Other.Rotation, VInverseRot ); // [Q(B)*Q(A)(-1)*T(A)*Q(A)*Q(B)(-1)] VInverseRot = VectorQuaternionInverse(VRotation); VectorRegister VQT = VectorQuaternionMultiply2(VRotation, Translation); VectorRegister VR = VectorQuaternionMultiply2(VQT, VInverseRot); // Translation = T(B)-S(B)/S(A) *[Q(B)*Q(A)(-1)*T(A)*Q(A)*Q(B)(-1)] VectorRegister VTranslation = VectorSet_W0(VectorSubtract(Other.Translation, VectorMultiply(VScale3D, VR))); Result.Scale3D = VScale3D; Result.Translation = VTranslation; Result.Rotation = VRotation; Result.DiagnosticCheckNaN_All(); #if DEBUG_INVERSE_TRANSFORM FMatrix AM = ToMatrixWithScale(); FMatrix BM = Other.ToMatrixWithScale(); Result.DebugEqualMatrix(AM.InverseFast() * BM); #endif return Result; }
/** Determines whether two bounding spheres intersect. */ FORCEINLINE bool AreSpheresNotIntersecting( const VectorRegister& A_XYZ, const VectorRegister& A_Radius, const VectorRegister& B_XYZ, const VectorRegister& B_Radius ) { const VectorRegister DeltaVector = VectorSubtract(A_XYZ,B_XYZ); const VectorRegister DistanceSquared = VectorDot3(DeltaVector,DeltaVector); const VectorRegister MaxDistance = VectorAdd(A_Radius,B_Radius); const VectorRegister MaxDistanceSquared = VectorMultiply(MaxDistance,MaxDistance); return !!VectorAnyGreaterThan(DistanceSquared,MaxDistanceSquared); }
const bool Test_Matrix_VectorMultiply() { const TMatrix4d kA4d{ 2.0, 0.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 0.0, 5.0}; const TVector4d kB4d{ 1.0, 1.0, 1.0, 1.0 }; const TVector4d kC4d{ 2.0, 3.0, 4.0, 5.0 }; const TMatrix4f kA4f{ 2.0f, 0.0f, 0.0f, 0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f, 0.0f, 4.0f, 0.0f, 0.0f, 0.0f, 0.0f, 5.0f}; const TVector4f kB4f{ 1.0f, 1.0f, 1.0f, 1.0f }; const TVector4f kC4f{ 2.0f, 3.0f, 4.0f, 5.0f }; const TMatrix3d kA3d{ 2.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 4.0}; const TVector3d kB3d{ 1.0, 1.0, 1.0 }; const TVector3d kC3d{ 2.0, 3.0, 4.0 }; const TMatrix3f kA3f{ 2.0f, 0.0f, 0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f, 4.0f}; const TVector3f kB3f{ 1.0f, 1.0f, 1.0f }; const TVector3f kC3f{ 2.0f, 3.0f, 4.0f }; const TMatrix2d kA2d{ 2.0, 0.0, 0.0, 3.0}; const TVector2d kB2d{ 1.0, 1.0 }; const TVector2d kC2d{ 2.0, 3.0 }; const TMatrix2f kA2f{ 2.0f, 0.0f, 0.0f, 3.0f}; const TVector2f kB2f{ 1.0f, 1.0f }; const TVector2f kC2f{ 2.0f, 3.0f }; const bool kbPass4d = Equal(kC4d, VectorMultiply(TVector4d(), kA4d, kB4d), s_kdEpsilon); const bool kbPass4f = Equal(kC4f, VectorMultiply(TVector4f(), kA4f, kB4f), s_kfEpsilon); const bool kbPass3d = Equal(kC3d, VectorMultiply(TVector3d(), kA3d, kB3d), s_kdEpsilon); const bool kbPass3f = Equal(kC3f, VectorMultiply(TVector3f(), kA3f, kB3f), s_kfEpsilon); const bool kbPass2d = Equal(kC2d, VectorMultiply(TVector2d(), kA2d, kB2d), s_kdEpsilon); const bool kbPass2f = Equal(kC2f, VectorMultiply(TVector2f(), kA2f, kB2f), s_kfEpsilon); return( kbPass4d && kbPass4f && kbPass3d && kbPass3f && kbPass2d && kbPass2f); }
/** * Set current transform and the relative to ParentTransform. * Equates to This = This->GetRelativeTransform(Parent), but saves the intermediate FTransform storage and copy. */ void FTransform::SetToRelativeTransform(const FTransform& ParentTransform) { // A * B(-1) = VQS(B)(-1) (VQS (A)) // // Scale = S(A)/S(B) // Rotation = Q(B)(-1) * Q(A) // Translation = 1/S(B) *[Q(B)(-1)*(T(A)-T(B))*Q(B)] // where A = this, B = Other #if DEBUG_INVERSE_TRANSFORM FMatrix AM = ToMatrixWithScale(); FMatrix BM = ParentTransform.ToMatrixWithScale(); #endif checkSlow(ParentTransform.IsRotationNormalized()); // Scale = S(A)/S(B) VectorRegister VSafeScale3D = VectorSet_W0(GetSafeScaleReciprocal(ParentTransform.Scale3D)); Scale3D = VectorMultiply(Scale3D, VSafeScale3D); //VQTranslation = ( ( T(A).X - T(B).X ), ( T(A).Y - T(B).Y ), ( T(A).Z - T(B).Z), 0.f ); VectorRegister VQTranslation = VectorSet_W0(VectorSubtract(Translation, ParentTransform.Translation)); // Translation = 1/S(B) *[Q(B)(-1)*(T(A)-T(B))*Q(B)] VectorRegister VInverseParentRot = VectorQuaternionInverse(ParentTransform.Rotation); VectorRegister VQT = VectorQuaternionMultiply2(VInverseParentRot, VQTranslation); VectorRegister VR = VectorQuaternionMultiply2(VQT, ParentTransform.Rotation); Translation = VectorMultiply(VR, VSafeScale3D); // Rotation = Q(B)(-1) * Q(A) Rotation = VectorQuaternionMultiply2(VInverseParentRot, Rotation ); DiagnosticCheckNaN_All(); #if DEBUG_INVERSE_TRANSFORM DebugEqualMatrix(AM * BM.InverseFast()); #endif }
//----------------------------------------------------------------------------- // Purpose: //----------------------------------------------------------------------------- void CNPC_Bug_Warrior::MeleeAttack( float distance, float damage, QAngle& viewPunch, Vector& shove ) { if ( GetEnemy() == NULL ) return; // Trace directly at my target Vector vStart = GetAbsOrigin(); vStart.z += WorldAlignSize().z * 0.5; Vector vecForward = (GetEnemy()->EyePosition() - vStart); VectorNormalize( vecForward ); Vector vEnd = vStart + (vecForward * distance ); // Use half the size of my target for the box Vector vecHalfTraceBox = (GetEnemy()->WorldAlignMaxs() - GetEnemy()->WorldAlignMins()) * 0.25; //NDebugOverlay::Box( vStart, -Vector(10,10,10), Vector(10,10,10), 0,255,0,20,1.0); //NDebugOverlay::Box( GetEnemy()->EyePosition(), -Vector(10,10,10), Vector(10,10,10), 255,255,255,20,1.0); CBaseEntity *pHurt = CheckTraceHullAttack( vStart, vEnd, -vecHalfTraceBox, vecHalfTraceBox, damage, DMG_SLASH ); if ( pHurt ) { CBasePlayer *pPlayer = ToBasePlayer( pHurt ); if ( pPlayer ) { //Kick the player angles pPlayer->ViewPunch( viewPunch ); Vector dir = pHurt->GetAbsOrigin() - GetAbsOrigin(); VectorNormalize(dir); QAngle angles; VectorAngles( dir, angles ); Vector forward, right; AngleVectors( angles, &forward, &right, NULL ); // Push the target back Vector vecImpulse; VectorMultiply( right, -shove[1], vecImpulse ); VectorMA( vecImpulse, -shove[0], forward, vecImpulse ); pHurt->ApplyAbsVelocityImpulse( vecImpulse ); } // Play a random attack hit sound EmitSound( "NPC_Bug_Warrior.AttackHit" ); } }
static void PerformNewCustomEffects( const Vector &vecOrigin, trace_t &tr, const Vector &shotDir, int iMaterial, int iScale, int nFlags ) { bool bNoFlecks = !r_drawflecks.GetBool(); if ( !bNoFlecks ) { bNoFlecks = ( ( nFlags & FLAGS_CUSTIOM_EFFECTS_NOFLECKS ) != 0 ); } // Compute the impact effect name const ImpactEffect_t &effect = s_pImpactEffect[ iMaterial - 'A' ]; const char *pImpactName = effect.m_pName; if ( bNoFlecks && effect.m_pNameNoFlecks ) { pImpactName = effect.m_pNameNoFlecks; } if ( !pImpactName ) return; CSmartPtr<CNewParticleEffect> pEffect = CNewParticleEffect::Create( NULL, pImpactName ); if ( !pEffect->IsValid() ) return; Vector vecReflect; float flDot = DotProduct( shotDir, tr.plane.normal ); VectorMA( shotDir, -2.0f * flDot, tr.plane.normal, vecReflect ); Vector vecShotBackward; VectorMultiply( shotDir, -1.0f, vecShotBackward ); Vector vecImpactPoint = ( tr.fraction != 1.0f ) ? tr.endpos : vecOrigin; // Other games round m_vOrigin to nearest integer, so I guess we can afford skipping this check. #ifdef HL2_CLIENT_DLL Assert( VectorsAreEqual( vecOrigin, tr.endpos, 1e-1 ) ); #endif SetImpactControlPoint( pEffect.GetObject(), 0, vecImpactPoint, tr.plane.normal, tr.m_pEnt ); SetImpactControlPoint( pEffect.GetObject(), 1, vecImpactPoint, vecReflect, tr.m_pEnt ); SetImpactControlPoint( pEffect.GetObject(), 2, vecImpactPoint, vecShotBackward, tr.m_pEnt ); pEffect->SetControlPoint( 3, Vector( iScale, iScale, iScale ) ); if ( pEffect->m_pDef->ReadsControlPoint( 4 ) ) { Vector vecColor; GetColorForSurface( &tr, &vecColor ); pEffect->SetControlPoint( 4, vecColor ); } }
//----------------------------------------------------------------------------- // Figure out where we enter the world //----------------------------------------------------------------------------- void CEnvHeadcrabCanister::ComputeWorldEntryPoint( Vector *pStartPosition, QAngle *pStartAngles, Vector *pStartDirection ) { SetupWorldModel(); Vector vecForward; GetVectors( &vecForward, NULL, NULL ); // Raycast up to the place where we should start from (start raycast slightly off the ground, // since it'll be buried in the ground oftentimes) trace_t tr; CTraceFilterWorldOnly filter; UTIL_TraceLine( GetAbsOrigin() + vecForward * 100, GetAbsOrigin() + vecForward * 10000, CONTENTS_SOLID, &filter, &tr ); *pStartPosition = tr.endpos; *pStartAngles = GetAbsAngles(); VectorMultiply( vecForward, -1.0f, *pStartDirection ); }
void CPlayerPositionProxy::OnBind( void *pC_BaseEntity ) { // Find the player speed.... C_BaseEntity* pPlayer = C_BasePlayer::GetLocalPlayer(); if (!pPlayer) return; // This is actually a vector... Assert( m_pResult ); Vector res; VectorMultiply( pPlayer->WorldSpaceCenter(), m_Factor, res ); m_pResult->SetVecValue( res.Base(), 3 ); if ( ToolsEnabled() ) { ToolFramework_RecordMaterialParams( GetMaterial() ); } }
bool FConvexVolume::IntersectSphere(const FVector& Origin,const float& Radius) const { bool Result = true; checkSlow(PermutedPlanes.Num() % 4 == 0); // Load the origin & radius VectorRegister Orig = VectorLoadFloat3(&Origin); VectorRegister VRadius = VectorLoadFloat1(&Radius); // Splat origin into 3 vectors VectorRegister OrigX = VectorReplicate(Orig, 0); VectorRegister OrigY = VectorReplicate(Orig, 1); VectorRegister OrigZ = VectorReplicate(Orig, 2); // Since we are moving straight through get a pointer to the data const FPlane* RESTRICT PermutedPlanePtr = (FPlane*)PermutedPlanes.GetData(); // Process four planes at a time until we have < 4 left for (int32 Count = 0; Count < PermutedPlanes.Num(); Count += 4) { // Load 4 planes that are already all Xs, Ys, ... VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr); PermutedPlanePtr++; // Calculate the distance (x * x) + (y * y) + (z * z) - w VectorRegister DistX = VectorMultiply(OrigX,PlanesX); VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX); VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY); VectorRegister Distance = VectorSubtract(DistZ,PlanesW); // Check for completely outside if (VectorAnyGreaterThan(Distance,VRadius)) { Result = false; break; } } return Result; }
void particleInterpolate(int i, float t, float *v) { particle_t *p1; particle_t *p2; VectorNew(moo); p1 = state.particleHistory + state.particleCount * state.currentFrame + i; if (state.currentFrame == state.historyFrames || state.historyFrames == 0 || state.mode & SM_RECORD) { VectorCopy(p1->pos, v); return; } p2 = state.particleHistory + state.particleCount * (state.currentFrame+1) + i; VectorSub(p1->pos, p2->pos, moo); VectorMultiply(moo, t, moo); VectorAdd(p1->pos, moo, v); }
//----------------------------------------------------------------------------- // Death volley //----------------------------------------------------------------------------- void CPropAPC::FireDying( ) { if ( m_flRocketTime > gpGlobals->curtime ) return; Vector vecRocketOrigin; GetRocketShootPosition( &vecRocketOrigin ); Vector vecDir; vecDir.Random( -1.0f, 1.0f ); if ( vecDir.z < 0.0f ) { vecDir.z *= -1.0f; } VectorNormalize( vecDir ); Vector vecVelocity; VectorMultiply( vecDir, ROCKET_SPEED * random->RandomFloat( 0.75f, 1.25f ), vecVelocity ); QAngle angles; VectorAngles( vecDir, angles ); CAPCMissile *pRocket = (CAPCMissile *) CAPCMissile::Create( vecRocketOrigin, angles, vecVelocity, this ); float flDeathTime = random->RandomFloat( 0.3f, 0.5f ); if ( random->RandomFloat( 0.0f, 1.0f ) < 0.3f ) { pRocket->ExplodeDelay( flDeathTime ); } else { pRocket->AugerDelay( flDeathTime ); } // Make erratic firing m_flRocketTime = gpGlobals->curtime + random->RandomFloat( DEATH_VOLLEY_MIN_FIRE_TIME, DEATH_VOLLEY_MAX_FIRE_TIME ); if ( --m_iRocketSalvoLeft <= 0 ) { CreateCorpse(); } }
void CBaseHelicopter::DoWashPushOnAirboat( CBaseEntity *pAirboat, const Vector &vecWashToAirboat, float flWashAmount ) { // For the airboat, simply produce a small roll and a push outwards. // But don't produce a roll if we're too rolled in that direction already. // Get the actual up direction vector Vector vecUp; pAirboat->GetVectors( NULL, NULL, &vecUp ); if ( vecUp.z < MAX_AIRBOAT_ROLL_COSANGLE ) return; // Compute roll direction so that we get pushed down on the side where the rotor wash is. Vector vecRollNormal; CrossProduct( vecWashToAirboat, Vector( 0, 0, 1 ), vecRollNormal ); // Project it into the plane of the roll normal VectorMA( vecUp, -DotProduct( vecUp, vecRollNormal ), vecRollNormal, vecUp ); VectorNormalize( vecUp ); // Compute a vector which is the max direction we can roll given the roll constraint Vector vecExtremeUp; VMatrix rot; MatrixBuildRotationAboutAxis( rot, vecRollNormal, MAX_AIRBOAT_ROLL_ANGLE ); MatrixGetColumn( rot, 2, &vecExtremeUp ); // Find the angle between how vertical we are and how vertical we should be float flCosDelta = DotProduct( vecExtremeUp, vecUp ); float flDelta = acos(flCosDelta) * 180.0f / M_PI; flDelta = clamp( flDelta, 0.0f, MAX_AIRBOAT_ROLL_ANGLE ); flDelta = SimpleSplineRemapVal( flDelta, 0.0f, MAX_AIRBOAT_ROLL_ANGLE, 0.0f, 1.0f ); float flForce = 12.0f * flWashAmount * flDelta; Vector vecWashOrigin; Vector vecForce; VectorMultiply( Vector( 0, 0, -1 ), flForce, vecForce ); VectorMA( pAirboat->GetAbsOrigin(), -200.0f, vecWashToAirboat, vecWashOrigin ); pAirboat->VPhysicsTakeDamage( CTakeDamageInfo( this, this, vecForce, vecWashOrigin, flWashAmount, DMG_BLAST ) ); }
static void ComputeAmbientFromSurface( dface_t* pFace, directlight_t* pSkylight, Vector& radcolor ) { texinfo_t* pTex = &texinfo[pFace->texinfo]; if (pTex) { // If we hit the sky, use the sky ambient if (pTex->flags & SURF_SKY) { if (pSkylight) { // add in sky ambient VectorDivide( pSkylight->light.intensity, 255.0f, radcolor ); } } else { VectorMultiply( radcolor, dtexdata[pTex->texdata].reflectivity, radcolor ); } } }