136 lines
2.9 KiB
C++
136 lines
2.9 KiB
C++
#include "StdAfx.h"
|
|
#include "GrpMath.h"
|
|
|
|
float CrossProduct2D(float x1, float y1, float x2, float y2)
|
|
{
|
|
return x1*y2-y1*x2;
|
|
}
|
|
|
|
bool IsInTriangle2D(float ax, float ay, float bx, float by, float cx, float cy, float tx, float ty)
|
|
{
|
|
float c1 = CrossProduct2D(bx-ax, by-ay, tx-ax, ty-ay);
|
|
float c2 = CrossProduct2D(cx-bx, cy-by, tx-bx, ty-by);
|
|
float c3 = CrossProduct2D(ax-cx, ay-cy, tx-cx, ty-cy);
|
|
|
|
if (c1 * c2 > 0.0f && c1 * c3 > 0.0f)
|
|
return true;
|
|
|
|
if (c1 * c2 * c3 == 0.0f)
|
|
{
|
|
if (tx < ax)
|
|
if (tx < bx)
|
|
if (tx < cx)
|
|
return false;
|
|
|
|
if (tx > ax)
|
|
if (tx > bx)
|
|
if (tx > cx)
|
|
return false;
|
|
|
|
if (ty < ay)
|
|
if (ty < by)
|
|
if (ty < cy)
|
|
return false;
|
|
|
|
if (ty > ay)
|
|
if (ty > by)
|
|
if (ty > cy)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
D3DXVECTOR3* D3DXVec3Rotation(D3DXVECTOR3* pvtOut, const D3DXVECTOR3* c_pvtSrc, const D3DXQUATERNION* c_pqtRot)
|
|
{
|
|
D3DXQUATERNION qtSrc(c_pvtSrc->x, c_pvtSrc->y, c_pvtSrc->z, 0);
|
|
D3DXQUATERNION qtRet;
|
|
D3DXQuaternionConjugate(&qtRet, c_pqtRot);
|
|
D3DXQuaternionMultiply(&qtRet, &qtSrc, &qtRet);
|
|
D3DXQuaternionMultiply(&qtRet, c_pqtRot, &qtRet);
|
|
|
|
pvtOut->x=qtRet.x;
|
|
pvtOut->y=qtRet.y;
|
|
pvtOut->z=qtRet.z;
|
|
|
|
return pvtOut;
|
|
}
|
|
|
|
|
|
|
|
void GetRotationFromMatrix(D3DXVECTOR3 * pRotation, const D3DXMATRIX * c_pMatrix)
|
|
{
|
|
float sx = c_pMatrix->_32;
|
|
float cx = sqrtf(1.0f - sx * sx);
|
|
|
|
if (cx < 0.00001f)
|
|
{
|
|
if (sx > 0)
|
|
pRotation->x = D3DX_PI / 2;
|
|
else
|
|
pRotation->x = -D3DX_PI / 2;
|
|
|
|
pRotation->y = atan2f(c_pMatrix->_31, c_pMatrix->_11);
|
|
pRotation->z = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
pRotation->x = atan2f(sx, cx);
|
|
pRotation->y = atan2f(-c_pMatrix->_31, c_pMatrix->_33);
|
|
pRotation->z = atan2f(-c_pMatrix->_12, c_pMatrix->_22);
|
|
}
|
|
}
|
|
|
|
void GetPivotAndRotationFromMatrix(D3DXMATRIX * pMatrix, D3DXVECTOR3 * pPivot, D3DXVECTOR3 * pRotation)
|
|
{
|
|
float sx = pMatrix->_32;
|
|
float cx = sqrtf(1.0f - sx * sx);
|
|
float x, y, z;
|
|
|
|
if (cx < 0.00001f)
|
|
{
|
|
if (sx > 0)
|
|
x = D3DX_PI / 2;
|
|
else
|
|
x = -D3DX_PI / 2;
|
|
|
|
y = atan2f(pMatrix->_31, pMatrix->_11);
|
|
z = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
x = atan2f(sx, cx);
|
|
y = atan2f(-pMatrix->_31, pMatrix->_33);
|
|
z = atan2f(-pMatrix->_12, pMatrix->_22);
|
|
}
|
|
|
|
pRotation->x = x;
|
|
pRotation->y = y;
|
|
pRotation->z = z;
|
|
|
|
pPivot->x = pMatrix->_41;
|
|
pPivot->y = pMatrix->_42;
|
|
pPivot->z = pMatrix->_43;
|
|
}
|
|
|
|
// NOTE : must be optimized!
|
|
void ExtractMovement(D3DXMATRIX * pTargetMatrix, D3DXMATRIX * pSourceMatrix)
|
|
{
|
|
D3DXVECTOR3 v3Pivot;
|
|
D3DXVECTOR3 v3Rotation;
|
|
GetPivotAndRotationFromMatrix(pSourceMatrix, &v3Pivot, &v3Rotation);
|
|
|
|
D3DXMATRIX matRotationX;
|
|
D3DXMatrixRotationX(&matRotationX, v3Rotation.x);
|
|
D3DXMATRIX matRotationY;
|
|
D3DXMatrixRotationY(&matRotationY, v3Rotation.y);
|
|
D3DXMATRIX matRotationZ;
|
|
D3DXMatrixRotationZ(&matRotationZ, v3Rotation.z);
|
|
|
|
D3DXMATRIX matTranslation;
|
|
D3DXMatrixTranslation(&matTranslation, v3Pivot.x, v3Pivot.y, v3Pivot.z);
|
|
|
|
*pTargetMatrix = matRotationX * matRotationY * matRotationZ * matTranslation;
|
|
} |