Commit 1ba4ead2 authored by D-AIRY's avatar D-AIRY

New math functions

parent ab0b5db2
......@@ -698,6 +698,11 @@ __forceinline SMVECTOR SMVectorMax(const SMVECTOR & V1, const SMVECTOR & V2)
return(v);
}
__forceinline SMVECTOR SMVectorAbs(const SMVECTOR &V)
{
return(SMVectorMax(V, -V));
}
__forceinline SMVECTOR SMVectorLerp(const SMVECTOR & V1, const SMVECTOR & V2, float t)
{
SMVECTOR L, S;
......@@ -2335,6 +2340,247 @@ __forceinline SMMATRIX SMMatrixReflect(const SMPLANE &_plane)
#define FLOAT_INF ((float)INFINITY)
__forceinline SMPLANE SMPlaneTransform(const SMPLANE &P, const SMMATRIX &M)
{
float fDeterminant;
SMMATRIX m = SMMatrixTranspose(SMMatrixInverse(&fDeterminant, M));
return(
float4(P.x) * m.r[0] +
float4(P.y) * m.r[1] +
float4(P.z) * m.r[2] +
float4(P.w) * m.r[3]
);
}
__forceinline SMPLANE SMPlaneNormalize(const SMPLANE &P)
{
float fInvLen = 1.0f / SMVector3Length(P);
return(P * fInvLen);
}
__forceinline bool SMPlaneIntersectAABB(const SMPLANE &P, const float3 &vMin, const float3 &vMax)
{
int i = 0;
i += (SMVector3Dot(P, vMin) > -P.w) ? 1 : 0;
i += (SMVector3Dot(P, float3(vMin.x, vMin.y, vMax.z)) > -P.w) ? 1 : 0;
i += (SMVector3Dot(P, float3(vMin.x, vMax.y, vMin.z)) > -P.w) ? 1 : 0;
i += (SMVector3Dot(P, float3(vMin.x, vMax.y, vMax.z)) > -P.w) ? 1 : 0;
i += (SMVector3Dot(P, float3(vMax.x, vMin.y, vMin.z)) > -P.w) ? 1 : 0;
i += (SMVector3Dot(P, float3(vMax.x, vMin.y, vMax.z)) > -P.w) ? 1 : 0;
i += (SMVector3Dot(P, float3(vMax.x, vMax.y, vMin.z)) > -P.w) ? 1 : 0;
i += (SMVector3Dot(P, vMax) > -P.w) ? 1 : 0;
return(i > 0 && i < 8);
}
__forceinline bool SMAABBIntersectAABB(const float3 &vMinA, const float3 &vMaxA, const float3 &vMinB, const float3 &vMaxB)
{
if(vMaxA.x < vMinB.x || vMinA.x > vMaxB.x) return(false);
if(vMaxA.y < vMinB.y || vMinA.y > vMaxB.y) return(false);
if(vMaxA.z < vMinB.z || vMinA.z > vMaxB.z) return(false);
return(true);
}
__forceinline bool SMVector4EqualEpsilon(const float4 &A, const float4 &B, float fEpsilon)
{
float4 vDelta = A - B;
SMVECTOR vTemp = SMVectorAbs(vDelta);
vTemp.mmv = _mm_cmple_ps(vTemp, float4(fEpsilon));
return((_mm_movemask_ps(vTemp) == 0xf) != 0);
}
__forceinline bool SMPlaneEqualEpsilon(const SMPLANE &A, const SMPLANE &B, float fEpsilon)
{
return(SMVector4EqualEpsilon(SMPlaneNormalize(A), SMPlaneNormalize(B), fEpsilon));
}
//##########################################################################
__forceinline float SMDistancePointAABB(const float3 &vPoint, const float3 &vMin, const float3 &vMax)
{
bool bXlo = vPoint.x < vMin.x;
bool bXhi = vPoint.x > vMax.x;
bool isInX = !bXlo && !bXhi;
bool bYlo = vPoint.y < vMin.y;
bool bYhi = vPoint.y > vMax.y;
bool isInY = !bYlo && !bYhi;
bool bZlo = vPoint.z < vMin.z;
bool bZhi = vPoint.z > vMax.z;
bool isInZ = !bZlo && !bZhi;
if(isInX && isInY && isInZ)
{
return(0.0f);
}
if(isInX && isInY)
{
if(bZlo)
{
return(vMin.z - vPoint.z);
}
else
{
return(vPoint.z - vMax.z);
}
}
if(isInX && isInZ)
{
if(bYlo)
{
return(vMin.y - vPoint.y);
}
else
{
return(vPoint.y - vMax.y);
}
}
if(isInY && isInZ)
{
if(bXlo)
{
return(vMin.x - vPoint.x);
}
else
{
return(vPoint.x - vMax.x);
}
}
if(isInX)
{
if(bYlo)
{
if(bZlo)
{
return(SMVector2Length(float2(vPoint.y, vPoint.z) - float2(vMin.y, vMin.z)));
}
else
{
return(SMVector2Length(float2(vPoint.y, vPoint.z) - float2(vMin.y, vMax.z)));
}
}
else
{
if(bZlo)
{
return(SMVector2Length(float2(vPoint.y, vPoint.z) - float2(vMax.y, vMin.z)));
}
else
{
return(SMVector2Length(float2(vPoint.y, vPoint.z) - float2(vMax.y, vMax.z)));
}
}
}
if(isInY)
{
if(bXlo)
{
if(bZlo)
{
return(SMVector2Length(float2(vPoint.x, vPoint.z) - float2(vMin.x, vMin.z)));
}
else
{
return(SMVector2Length(float2(vPoint.x, vPoint.z) - float2(vMin.x, vMax.z)));
}
}
else
{
if(bZlo)
{
return(SMVector2Length(float2(vPoint.x, vPoint.z) - float2(vMax.x, vMin.z)));
}
else
{
return(SMVector2Length(float2(vPoint.x, vPoint.z) - float2(vMax.x, vMax.z)));
}
}
}
if(isInZ)
{
if(bXlo)
{
if(bYlo)
{
return(SMVector2Length(float2(vPoint.x, vPoint.y) - float2(vMin.x, vMin.y)));
}
else
{
return(SMVector2Length(float2(vPoint.x, vPoint.y) - float2(vMin.x, vMax.y)));
}
}
else
{
if(bYlo)
{
return(SMVector2Length(float2(vPoint.x, vPoint.y) - float2(vMax.x, vMin.y)));
}
else
{
return(SMVector2Length(float2(vPoint.x, vPoint.y) - float2(vMax.x, vMax.y)));
}
}
}
// else
{
if(bXlo)
{
if(bYlo)
{
if(bZlo)
{
return(SMVector3Length(vPoint - float3(vMin.x, vMin.y, vMin.z)));
}
else
{
return(SMVector3Length(vPoint - float3(vMin.x, vMin.y, vMax.z)));
}
}
else
{
if(bZlo)
{
return(SMVector3Length(vPoint - float3(vMin.x, vMax.y, vMin.z)));
}
else
{
return(SMVector3Length(vPoint - float3(vMin.x, vMax.y, vMax.z)));
}
}
}
else
{
if(bYlo)
{
if(bZlo)
{
return(SMVector3Length(vPoint - float3(vMax.x, vMin.y, vMin.z)));
}
else
{
return(SMVector3Length(vPoint - float3(vMax.x, vMin.y, vMax.z)));
}
}
else
{
if(bZlo)
{
return(SMVector3Length(vPoint - float3(vMax.x, vMax.y, vMin.z)));
}
else
{
return(SMVector3Length(vPoint - float3(vMax.x, vMax.y, vMax.z)));
}
}
}
}
}
//##########################################################################
//! возвращает нормаль треугольника
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment