void dgCollisionScene::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
dgVector origin (matrix.TransformVector(m_boxOrigin));
dgVector size (m_boxSize.m_x * dgAbsf(matrix[0][0]) + m_boxSize.m_y * dgAbsf(matrix[1][0]) + m_boxSize.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,
m_boxSize.m_x * dgAbsf(matrix[0][1]) + m_boxSize.m_y * dgAbsf(matrix[1][1]) + m_boxSize.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,
m_boxSize.m_x * dgAbsf(matrix[0][2]) + m_boxSize.m_y * dgAbsf(matrix[1][2]) + m_boxSize.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
dgFloat32 (0.0f));
p0 = origin - size;
p1 = origin + size;
#ifdef DG_DEBUG_AABB
dgInt32 i;
dgVector q0;
dgVector q1;
dgMatrix trans (matrix.Transpose());
for (i = 0; i < 3; i ++) {
q0[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i].Scale (-1.0f)))[i];
q1[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i]))[i];
}
dgVector err0 (p0 - q0);
dgVector err1 (p1 - q1);
dgFloat32 err;
err = GetMax (size.m_x, size.m_y, size.m_z) * 0.5f;
_ASSERTE ((err0 % err0) < err);
_ASSERTE ((err1 % err1) < err);
#endif
}
bool dgCollisionConvexHull::OOBBTest (const dgMatrix& matrix, const dgCollisionConvex* const shape, void* const cacheOrder) const
{
bool ret;
_ASSERTE (cacheOrder);
ret = dgCollisionConvex::OOBBTest (matrix, shape, cacheOrder);
if (ret) {
const dgConvexSimplexEdge* const* faceArray = m_faceArray;
dgCollisionBoundPlaneCache* const cache = (dgCollisionBoundPlaneCache*)cacheOrder;
for (dgInt32 i = 0; i < dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)); i ++) {
dgFloat32 dist;
const dgPlane& plane = cache->m_planes[i];
if ((plane % plane) > dgFloat32 (0.0f)) {
dgVector dir (matrix.UnrotateVector(plane.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
dist = plane.Evalue (p);
if (dist > dgFloat32 (0.1f)){
return false;
}
}
}
for (dgInt32 i = 0; i < m_boundPlanesCount; i ++) {
dgInt32 i0;
dgInt32 i1;
dgInt32 i2;
dgFloat32 dist;
const dgConvexSimplexEdge* const face = faceArray[i];
i0 = face->m_prev->m_vertex;
i1 = face->m_vertex;
i2 = face->m_next->m_vertex;
const dgVector& p0 = m_vertex[i0];
dgVector normal ((m_vertex[i1] - p0) * (m_vertex[i2] - p0));
normal = normal.Scale (dgFloat32 (1.0f) / dgSqrt (normal % normal));
dgVector dir (matrix.UnrotateVector(normal.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
//_ASSERTE ((normal % (m_boxOrigin - p0)) < 0.0f);
dist = normal % (p - p0);
if (dist > dgFloat32 (0.1f)){
for (dgInt32 j = 0; j < (dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)) - 1); j ++) {
cache->m_planes[j + 1] = cache->m_planes[j];
}
cache->m_planes[1] = dgPlane (normal, - (normal % p0));
return false;
}
}
}
return ret;
}
void dgCollisionSphere::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
dgTriplex pool[1024 * 2];
dgVector tmpVectex[1024 * 2];
dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));
dgInt32 i = 3;
dgInt32 count = 0;
TesselateTriangle (i, p4, p0, p2, count, tmpVectex);
TesselateTriangle (i, p4, p2, p1, count, tmpVectex);
TesselateTriangle (i, p4, p1, p3, count, tmpVectex);
TesselateTriangle (i, p4, p3, p0, count, tmpVectex);
TesselateTriangle (i, p5, p2, p0, count, tmpVectex);
TesselateTriangle (i, p5, p1, p2, count, tmpVectex);
TesselateTriangle (i, p5, p3, p1, count, tmpVectex);
TesselateTriangle (i, p5, p0, p3, count, tmpVectex);
for (dgInt32 i = 0; i < count; i ++) {
tmpVectex[i] = tmpVectex[i].Scale4 (m_radius);
}
//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &tmpVectex[0].m_x, sizeof (dgVector), count);
for (dgInt32 i = 0; i < count; i += 3) {
callback (userData, 3, &pool[i].m_x, 0);
}
}
// Compute axis aligned box
static void BoundingBox (const dgMatrix &Mat, const hacd::HaF32 vertex[], hacd::HaI32 vertexCount, hacd::HaI32 stride, dgVector &min, dgVector &max)
{
hacd::HaF32 xmin = hacd::HaF32 (1.0e10f);
hacd::HaF32 ymin = hacd::HaF32 (1.0e10f);
hacd::HaF32 zmin = hacd::HaF32 (1.0e10f);
hacd::HaF32 xmax = hacd::HaF32 (-1.0e10f);
hacd::HaF32 ymax = hacd::HaF32 (-1.0e10f);
hacd::HaF32 zmax = hacd::HaF32 (-1.0e10f);
const hacd::HaF32* ptr = vertex;
for (hacd::HaI32 i = 0 ; i < vertexCount; i ++ ) {
dgVector tmp (ptr[0], ptr[1], ptr[2], hacd::HaF32 (0.0f));
ptr += stride;
tmp = Mat.UnrotateVector (tmp);
if (tmp.m_x < xmin) xmin = tmp.m_x;
if (tmp.m_y < ymin) ymin = tmp.m_y;
if (tmp.m_z < zmin) zmin = tmp.m_z;
if (tmp.m_x > xmax) xmax = tmp.m_x;
if (tmp.m_y > ymax) ymax = tmp.m_y;
if (tmp.m_z > zmax) zmax = tmp.m_z;
}
min = dgVector (xmin, ymin, zmin, hacd::HaF32 (0.0f));
max = dgVector (xmax, ymax, zmax, hacd::HaF32 (0.0f));
}
static void BoundingBox (const dgMatrix &matrix, const hacd::HaF32 vertex[], hacd::HaI32 stride, const hacd::HaI32 index[], hacd::HaI32 indexCount, dgVector &min, dgVector &max)
{
hacd::HaF32 xmin = hacd::HaF32 (1.0e10f);
hacd::HaF32 ymin = hacd::HaF32 (1.0e10f);
hacd::HaF32 zmin = hacd::HaF32 (1.0e10f);
hacd::HaF32 xmax = hacd::HaF32 (-1.0e10f);
hacd::HaF32 ymax = hacd::HaF32 (-1.0e10f);
hacd::HaF32 zmax = hacd::HaF32 (-1.0e10f);
const hacd::HaF32* const ptr = vertex;
for (hacd::HaI32 j = 0 ; j < indexCount; j ++ ) {
hacd::HaI32 i = index[j] * stride;
dgVector tmp (ptr[i + 0], ptr[i + 1], ptr[i + 2], hacd::HaF32 (0.0f));
tmp = matrix.UnrotateVector (tmp);
if (tmp.m_x < xmin) xmin = tmp.m_x;
if (tmp.m_y < ymin) ymin = tmp.m_y;
if (tmp.m_z < zmin) zmin = tmp.m_z;
if (tmp.m_x > xmax) xmax = tmp.m_x;
if (tmp.m_y > ymax) ymax = tmp.m_y;
if (tmp.m_z > zmax) zmax = tmp.m_z;
}
min = dgVector (xmin, ymin, zmin, hacd::HaF32 (0.0f));
max = dgVector (xmax, ymax, zmax, hacd::HaF32 (0.0f));
}
dgInt32 dgBroadPhaseDefault::ConvexCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32& timeToImpact, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const
{
dgInt32 totalCount = 0;
if (m_rootNode) {
dgVector boxP0;
dgVector boxP1;
dgAssert(matrix.TestOrthogonal());
shape->CalcAABB(matrix, boxP0, boxP1);
dgFloat32 distance[DG_BROADPHASE_MAX_STACK_DEPTH];
const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH];
dgVector velocA((target - matrix.m_posit) & dgVector::m_triplexMask);
dgVector velocB(dgFloat32(0.0f));
dgFastRayTest ray(dgVector(dgFloat32(0.0f)), velocA);
dgVector minBox(m_rootNode->m_minBox - boxP1);
dgVector maxBox(m_rootNode->m_maxBox - boxP0);
stackPool[0] = m_rootNode;
distance[0] = ray.BoxIntersect(minBox, maxBox);
totalCount = dgBroadPhase::ConvexCast(stackPool, distance, 1, velocA, velocB, ray, shape, matrix, target, timeToImpact, prefilter, userData, info, maxContacts, threadIndex);
}
return totalCount;
}
void dgCollisionDeformableMesh::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
dgVector origin (matrix.TransformVector(m_boxOrigin));
dgVector size (matrix.m_front.Abs().Scale4(m_boxSize.m_x) + matrix.m_up.Abs().Scale4(m_boxSize.m_y) + matrix.m_right.Abs().Scale4(m_boxSize.m_z));
p0 = (origin - size) & dgVector::m_triplexMask;
p1 = (origin + size) & dgVector::m_triplexMask;
}
void dgCollisionDeformableSolidMesh::SetMatrix(const dgMatrix& matrix)
{
dgAssert (m_myBody);
//dgMatrix globalMatrix (GetBody()->GetCollision()->GetLocalMatrix() * matrix);
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
m_particles.m_posit[i] = m_shapePosit[i];
m_posit[i] = matrix.TransformVector(m_shapePosit[i]) & dgVector::m_triplexMask;
}
}
void dgCollisionChamferCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;
dgInt32 slices = 12;
dgInt32 brakes = 24;
dgFloat32 sliceAngle = dgFloat32 (0.0f);
dgFloat32 sliceStep = dgPI / slices;
dgFloat32 breakStep = dgPI2 / brakes;
dgTriplex pool[24 * (12 + 1)];
dgMatrix rot (dgPitchMatrix (breakStep));
dgInt32 index = 0;
for (dgInt32 j = 0; j <= slices; j ++) {
dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f));
sliceAngle += sliceStep;
for (dgInt32 i = 0; i < brakes; i ++) {
pool[index].m_x = p0.m_x;
pool[index].m_y = p0.m_y;
pool[index].m_z = p0.m_z;
index ++;
p0 = rot.UnrotateVector (p0);
}
}
//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * (12 + 1));
dgTriplex face[32];
index = 0;
for (dgInt32 j = 0; j < slices; j ++) {
dgInt32 index0 = index + brakes - 1;
for (dgInt32 i = 0; i < brakes; i ++) {
face[0] = pool[index];
face[1] = pool[index0];
face[2] = pool[index0 + brakes];
face[3] = pool[index + brakes];
index0 = index;
index ++;
callback (userData, 4, &face[0].m_x, 0);
}
}
for (dgInt32 i = 0; i < brakes; i ++) {
face[i] = pool[i];
}
callback (userData, 24, &face[0].m_x, 0);
for (dgInt32 i = 0; i < brakes; i ++) {
face[i] = pool[brakes * (slices + 1) - i - 1];
}
callback (userData, 24, &face[0].m_x, 0);
}
dgMatrix::dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis)
{
dgMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale4 (scale[0]);
scaledAxis[1] = stretchAxis[1].Scale4 (scale[1]);
scaledAxis[2] = stretchAxis[2].Scale4 (scale[2]);
scaledAxis[3] = stretchAxis[3];
*this = stretchAxis.Transpose() * scaledAxis * transformMatrix;
}
dgQuaternion::dgQuaternion (const dgMatrix& matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dgFloat32 trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
if (trace > dgFloat32(0.0f)) {
trace = dgSqrt (trace + dgFloat32(1.0f));
m_q0 = dgFloat32 (0.5f) * trace;
trace = dgFloat32 (0.5f) / trace;
m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dgFloat32(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dgSqrt (trace);
dgFloat32* const ptr = &m_q1;
ptr[i] = dgFloat32 (0.5f) * trace;
trace = dgFloat32 (0.5f) / trace;
m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#ifdef _DEBUG
dgMatrix tmp (*this, matrix.m_posit);
dgMatrix unitMatrix (tmp * matrix.Inverse());
for (dgInt32 i = 0; i < 4; i ++) {
dgFloat32 err = dgAbsf (unitMatrix[i][i] - dgFloat32(1.0f));
dgAssert (err < dgFloat32 (1.0e-2f));
}
dgFloat32 err = dgAbsf (DotProduct(*this) - dgFloat32(1.0f));
dgAssert (err < dgFloat32(dgEPSILON * 100.0f));
#endif
}
void dgCollisionDeformableMesh::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
const dgVector* const particlePosit = m_particles.m_posit;
for (dgInt32 i = 0; i < m_trianglesCount; i ++ ) {
dgTriplex points[3];
for (dgInt32 j = 0; j < 3; j ++) {
dgInt32 index = m_indexList[i * 3 + j];
dgVector p (matrix.TransformVector(particlePosit[index]));
points[j].m_x = p.m_x;
points[j].m_y = p.m_y;
points[j].m_z = p.m_z;
}
callback (userData, 3, &points[0].m_x, 0);
}
}
static void BoundingBox (
const dgMatrix &matrix,
const dgFloat32 vertex[],
dgInt32 stride,
const dgInt32 index[],
dgInt32 indexCount,
dgVector &min,
dgVector &max)
{
dgInt32 i;
dgInt32 j;
const dgFloat32 *ptr;
dgFloat32 xmin;
dgFloat32 xmax;
dgFloat32 ymin;
dgFloat32 ymax;
dgFloat32 zmin;
dgFloat32 zmax;
xmin = dgFloat32 (1.0e10f);
ymin = dgFloat32 (1.0e10f);
zmin = dgFloat32 (1.0e10f);
xmax = dgFloat32 (-1.0e10f);
ymax = dgFloat32 (-1.0e10f);
zmax = dgFloat32 (-1.0e10f);
ptr = vertex;
for (j = 0 ; j < indexCount; j ++ ) {
i = index[j] * stride;
dgVector tmp (ptr[i + 0], ptr[i + 1], ptr[i + 2], dgFloat32 (0.0f));
tmp = matrix.UnrotateVector (tmp);
if (tmp.m_x < xmin) xmin = tmp.m_x;
if (tmp.m_y < ymin) ymin = tmp.m_y;
if (tmp.m_z < zmin) zmin = tmp.m_z;
if (tmp.m_x > xmax) xmax = tmp.m_x;
if (tmp.m_y > ymax) ymax = tmp.m_y;
if (tmp.m_z > zmax) zmax = tmp.m_z;
}
min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
}
// Compute axis aligned box
static void BoundingBox (
const dgMatrix &Mat,
const dgFloat32 vertex[],
dgInt32 vertexCount,
dgInt32 stride,
dgVector &min,
dgVector &max)
{
dgInt32 i;
const dgFloat32 *ptr;
dgFloat32 xmin;
dgFloat32 xmax;
dgFloat32 ymin;
dgFloat32 ymax;
dgFloat32 zmin;
dgFloat32 zmax;
xmin = dgFloat32 (1.0e10f);
ymin = dgFloat32 (1.0e10f);
zmin = dgFloat32 (1.0e10f);
xmax = dgFloat32 (-1.0e10f);
ymax = dgFloat32 (-1.0e10f);
zmax = dgFloat32 (-1.0e10f);
ptr = vertex;
for (i = 0 ; i < vertexCount; i ++ ) {
dgVector tmp (ptr[0], ptr[1], ptr[2], dgFloat32 (0.0f));
ptr += stride;
tmp = Mat.UnrotateVector (tmp);
if (tmp.m_x < xmin) xmin = tmp.m_x;
if (tmp.m_y < ymin) ymin = tmp.m_y;
if (tmp.m_z < zmin) zmin = tmp.m_z;
if (tmp.m_x > xmax) xmax = tmp.m_x;
if (tmp.m_y > ymax) ymax = tmp.m_y;
if (tmp.m_z > zmax) zmax = tmp.m_z;
}
min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
}
void dgCollisionTaperedCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
dgTriplex pool[24 * 2];
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < 24; i ++) {
dgFloat32 z = dgSin (angle);
dgFloat32 y = dgCos (angle);
pool[i].m_x = - m_height;
pool[i].m_y = y * m_radio1;
pool[i].m_z = z * m_radio1;
pool[i + 24].m_x = m_height;
pool[i + 24].m_y = y * m_radio0;
pool[i + 24].m_z = z * m_radio0;
angle += dgPI2 / dgFloat32 (24.0f);
}
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * 2);
dgTriplex face[24];
dgInt32 j = 24 - 1;
for (dgInt32 i = 0; i < 24; i ++) {
face[0] = pool[j];
face[1] = pool[i];
face[2] = pool[i + 24];
face[3] = pool[j + 24];
j = i;
callback (userData, 4, &face[0].m_x, 0);
}
for (dgInt32 i = 0; i < 24; i ++) {
face[i] = pool[24 - 1 - i];
}
callback (userData, 24, &face[0].m_x, 0);
for (dgInt32 i = 0; i < 24; i ++) {
face[i] = pool[i + 24];
}
callback (userData, 24, &face[0].m_x, 0);
}
dgVector dgBilateralConstraint::CalculateGlobalMatrixAndAngle (dgMatrix& globalMatrix0, dgMatrix& globalMatrix1) const
{
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& body0Matrix = m_body0->GetMatrix();
const dgMatrix& body1Matrix = m_body1->GetMatrix();
globalMatrix0 = m_localMatrix0 * body0Matrix;
globalMatrix1 = m_localMatrix1 * body1Matrix;
dgMatrix relMatrix (globalMatrix1 * globalMatrix0.Inverse());
dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_front % relMatrix.m_front)) < 1.0e-5f);
dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_up % relMatrix.m_up)) < 1.0e-5f);
dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_right % relMatrix.m_right)) < 1.0e-5f);
dgVector euler0;
dgVector euler1;
relMatrix.CalcPitchYawRoll (euler0, euler1);
return euler0;
}
void dgCollisionCone::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;
#define NUMBER_OF_DEBUG_SEGMENTS 40
dgTriplex pool[NUMBER_OF_DEBUG_SEGMENTS + 1];
dgTriplex face[NUMBER_OF_DEBUG_SEGMENTS];
dgFloat32 angle = dgFloat32 (0.0f);
for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
dgFloat32 z = dgSin (angle) * m_radius;
dgFloat32 y = dgCos (angle) * m_radius;
pool[i].m_x = -m_height;
pool[i].m_y = y;
pool[i].m_z = z;
angle += dgPI2 / dgFloat32 (NUMBER_OF_DEBUG_SEGMENTS);
}
pool[NUMBER_OF_DEBUG_SEGMENTS].m_x = m_height;
pool[NUMBER_OF_DEBUG_SEGMENTS].m_y = dgFloat32 (0.0f);
pool[NUMBER_OF_DEBUG_SEGMENTS].m_z = dgFloat32 (0.0f);
matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), NUMBER_OF_DEBUG_SEGMENTS + 1);
dgInt32 j = NUMBER_OF_DEBUG_SEGMENTS - 1;
for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
face[0] = pool[j];
face[1] = pool[i];
face[2] = pool[NUMBER_OF_DEBUG_SEGMENTS];
j = i;
callback (userData, 3, &face[0].m_x, 0);
}
for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
face[i] = pool[NUMBER_OF_DEBUG_SEGMENTS - 1 - i];
}
callback (userData, NUMBER_OF_DEBUG_SEGMENTS, &face[0].m_x, 0);
}
void dgCollisionConvexHull::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
// dgTriplex tmp[1024 * 4];
// matrix.TransformTriplex (&tmp[0].m_x, sizeof (dgTriplex), &m_vertex[0].m_x, sizeof (dgVector), m_vertexCount);
dgTriplex vertex[256];
for (dgInt32 i = 0; i < m_faceCount; i ++) {
dgConvexSimplexEdge* const face = m_faceArray[i];
dgConvexSimplexEdge* ptr = face;
dgInt32 count = 0;
do {
//vertex[count] = tmp[ptr->m_vertex];
vertex[count].m_x = m_vertex[ptr->m_vertex].m_x;
vertex[count].m_y = m_vertex[ptr->m_vertex].m_y;
vertex[count].m_z = m_vertex[ptr->m_vertex].m_z;
count ++;
dgAssert (count < sizeof (vertex)/ sizeof (vertex[0]));
ptr = ptr->m_next;
} while (ptr != face);
matrix.TransformTriplex (&vertex[0].m_x, sizeof (dgTriplex), &vertex[0].m_x, sizeof (dgTriplex), count);
callback (userData, count, &vertex[0].m_x, 0);
}
}
dgVector dgBilateralConstraint::CalculateGlobalMatrixAndAngle (dgMatrix& globalMatrix0, dgMatrix& globalMatrix1) const
{
_ASSERTE (m_body0);
_ASSERTE (m_body1);
const dgMatrix& body0Matrix = m_body0->GetMatrix();
const dgMatrix& body1Matrix = m_body1->GetMatrix();
// dgMatrix body1Matrix (dgGetIdentityMatrix());
// if (m_body1) {
// body1Matrix = m_body1->GetMatrix();
// }
globalMatrix0 = m_localMatrix0 * body0Matrix;
globalMatrix1 = m_localMatrix1 * body1Matrix;
dgMatrix relMatrix (globalMatrix1 * globalMatrix0.Inverse());
_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_front % relMatrix.m_front)) < 1.0e-5f);
_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_up % relMatrix.m_up)) < 1.0e-5f);
_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_right % relMatrix.m_right)) < 1.0e-5f);
// _ASSERTE ((relMatrix.m_posit % relMatrix.m_posit) < 1.0e-3f);
return relMatrix.CalcPitchYawRoll ();
}
void dgCollisionTaperedCapsule::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
#define POWER 2
dgVector tmpVectex[1024 * 2];
dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));
dgInt32 count = 0;
TesselateTriangle (POWER, p0, p2, p4, count, tmpVectex);
TesselateTriangle (POWER, p0, p4, p3, count, tmpVectex);
TesselateTriangle (POWER, p0, p3, p5, count, tmpVectex);
TesselateTriangle (POWER, p0, p5, p2, count, tmpVectex);
TesselateTriangle (POWER, p1, p4, p2, count, tmpVectex);
TesselateTriangle (POWER, p1, p3, p4, count, tmpVectex);
TesselateTriangle (POWER, p1, p5, p3, count, tmpVectex);
TesselateTriangle (POWER, p1, p2, p5, count, tmpVectex);
dgFloat32 scale = m_radio1 / m_radio0;
dgVector edgeP0(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector edgeP1(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < count; i += 3) {
dgVector face0[4];
dgVector face1[4];
dgInt32 n0 = 0;
dgInt32 n1 = 0;
dgVector p0 (tmpVectex[i + 2]);
for (dgInt32 j = 0; j < 3; j ++) {
dgVector p1 (tmpVectex[i + j]);
if (p1.m_x > m_clip0) {
if (p0.m_x < m_clip0) {
dgFloat32 t = (m_clip0 - p0.m_x) / (p1.m_x - p0.m_x);
edgeP0 = p0 + (p1 - p0).Scale3 (t);
edgeP0.m_x = m_clip0;
face0[n0] = edgeP0;
face0[n0].m_x += m_height;
n0 ++;
face1[n1] = edgeP0.Scale3 (scale);
face1[n1].m_x -= m_height;
n1 ++;
}
face0[n0] = p1;
face0[n0].m_x += m_height;
n0 ++;
} else {
if (p0.m_x > m_clip0) {
dgFloat32 t = (m_clip0 - p0.m_x) / (p1.m_x - p0.m_x);
edgeP1 = p0 + (p1 - p0).Scale3 (t);
edgeP1.m_x = m_clip0;
face0[n0] = edgeP1;
face0[n0].m_x += m_height;
n0 ++;
face1[n1] = edgeP1.Scale3 (scale);
face1[n1].m_x -= m_height;
n1 ++;
}
face1[n1] = p1.Scale3 (scale);;
face1[n1].m_x -= m_height;
n1 ++;
}
p0 = p1;
}
dgTriplex face[4];
if (n0) {
matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face0[0].m_x, sizeof (dgVector), n0);
callback (userData, n0, &face[0].m_x, 0);
}
if (n1) {
matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face1[0].m_x, sizeof (dgVector), n1);
callback (userData, n1, &face[0].m_x, 0);
}
if (n0 && n1) {
face0[0] = edgeP0;
face0[1] = edgeP1;
face0[2] = edgeP1.Scale3 (scale);
face0[3] = edgeP0.Scale3 (scale);
face0[0].m_x += m_height;
face0[1].m_x += m_height;
face0[2].m_x -= m_height;
face0[3].m_x -= m_height;
dgPlane plane (face0[0], face0[1], face0[2]);
if (plane.m_w >= dgFloat32 (0.0f)) {
dgAssert (0);
}
matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face0[0].m_x, sizeof (dgVector), 4);
callback (userData, 4, &face[0].m_x, 0);
}
}
}
void dgPolygonSoupDatabaseBuilder::AddMesh (const hacd::HaF32* const vertex, hacd::HaI32 vertexCount, hacd::HaI32 strideInBytes, hacd::HaI32 faceCount,
const hacd::HaI32* const faceArray, const hacd::HaI32* const indexArray, const hacd::HaI32* const faceTagsData, const dgMatrix& worldMatrix)
{
hacd::HaI32 faces[256];
hacd::HaI32 pool[2048];
m_vertexPoints[m_vertexCount + vertexCount].m_x = hacd::HaF64 (0.0f);
dgBigVector* const vertexPool = &m_vertexPoints[m_vertexCount];
worldMatrix.TransformTriplex (&vertexPool[0].m_x, sizeof (dgBigVector), vertex, strideInBytes, vertexCount);
for (hacd::HaI32 i = 0; i < vertexCount; i ++) {
vertexPool[i].m_w = hacd::HaF64 (0.0f);
}
hacd::HaI32 totalIndexCount = faceCount;
for (hacd::HaI32 i = 0; i < faceCount; i ++) {
totalIndexCount += faceArray[i];
}
m_vertexIndex[m_indexCount + totalIndexCount] = 0;
m_faceVertexCount[m_faceCount + faceCount] = 0;
hacd::HaI32 k = 0;
for (hacd::HaI32 i = 0; i < faceCount; i ++) {
hacd::HaI32 count = faceArray[i];
for (hacd::HaI32 j = 0; j < count; j ++) {
hacd::HaI32 index = indexArray[k];
pool[j] = index + m_vertexCount;
k ++;
}
hacd::HaI32 convexFaces = 0;
if (count == 3) {
convexFaces = 1;
dgBigVector p0 (m_vertexPoints[pool[2]]);
for (hacd::HaI32 i = 0; i < 3; i ++) {
dgBigVector p1 (m_vertexPoints[pool[i]]);
dgBigVector edge (p1 - p0);
hacd::HaF64 mag2 = edge % edge;
if (mag2 < hacd::HaF32 (1.0e-6f)) {
convexFaces = 0;
}
p0 = p1;
}
if (convexFaces) {
dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
dgBigVector normal (edge0 * edge1);
hacd::HaF64 mag2 = normal % normal;
if (mag2 < hacd::HaF32 (1.0e-8f)) {
convexFaces = 0;
}
}
if (convexFaces) {
faces[0] = 3;
}
} else {
convexFaces = AddConvexFace (count, pool, faces);
}
hacd::HaI32 index = 0;
for (hacd::HaI32 k = 0; k < convexFaces; k ++) {
hacd::HaI32 count = faces[k];
m_vertexIndex[m_indexCount] = faceTagsData[i];
m_indexCount ++;
for (hacd::HaI32 j = 0; j < count; j ++) {
m_vertexIndex[m_indexCount] = pool[index];
index ++;
m_indexCount ++;
}
m_faceVertexCount[m_faceCount] = count + 1;
m_faceCount ++;
}
}
m_vertexCount += vertexCount;
m_run -= vertexCount;
if (m_run <= 0) {
PackArray();
}
}
DG_INLINE void dgSolver::BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide)
{
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 count = jointInfo->m_pairCount;
const dgDynamicBody* const body0 = (dgDynamicBody*)m_bodyArray[m0].m_body;
const dgDynamicBody* const body1 = (dgDynamicBody*)m_bodyArray[m1].m_body;
const bool isBilateral = jointInfo->m_joint->IsBilateral();
const dgMatrix invInertia0 = body0->m_invWorldInertiaMatrix;
const dgMatrix invInertia1 = body1->m_invWorldInertiaMatrix;
const dgVector invMass0(body0->m_invMass[3]);
const dgVector invMass1(body1->m_invMass[3]);
dgSoaFloat force0(m_soaZero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = dgSoaFloat(body0->m_externalForce, body0->m_externalTorque);
}
dgSoaFloat force1(m_soaZero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = dgSoaFloat(body1->m_externalForce, body1->m_externalTorque);
}
jointInfo->m_preconditioner0 = dgFloat32(1.0f);
jointInfo->m_preconditioner1 = dgFloat32(1.0f);
if ((invMass0.GetScalar() > dgFloat32(0.0f)) && (invMass1.GetScalar() > dgFloat32(0.0f)) && !(body0->GetSkeleton() && body1->GetSkeleton())) {
const dgFloat32 mass0 = body0->GetMass().m_w;
const dgFloat32 mass1 = body1->GetMass().m_w;
if (mass0 > (DG_DIAGONAL_PRECONDITIONER * mass1)) {
jointInfo->m_preconditioner0 = mass0 / (mass1 * DG_DIAGONAL_PRECONDITIONER);
} else if (mass1 > (DG_DIAGONAL_PRECONDITIONER * mass0)) {
jointInfo->m_preconditioner1 = mass1 / (mass0 * DG_DIAGONAL_PRECONDITIONER);
}
}
const dgFloat32 forceImpulseScale = dgFloat32(1.0f);
const dgSoaFloat weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0);
const dgSoaFloat weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0);
for (dgInt32 i = 0; i < count; i++) {
dgLeftHandSide* const row = &leftHandSide[index + i];
dgRightHandSide* const rhs = &rightHandSide[index + i];
row->m_JMinv.m_jacobianM0.m_linear = row->m_Jt.m_jacobianM0.m_linear * invMass0;
row->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(row->m_Jt.m_jacobianM0.m_angular);
row->m_JMinv.m_jacobianM1.m_linear = row->m_Jt.m_jacobianM1.m_linear * invMass1;
row->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(row->m_Jt.m_jacobianM1.m_angular);
const dgSoaFloat& JMinvM0 = (dgSoaFloat&)row->m_JMinv.m_jacobianM0;
const dgSoaFloat& JMinvM1 = (dgSoaFloat&)row->m_JMinv.m_jacobianM1;
const dgSoaFloat tmpAccel((JMinvM0 * force0).MulAdd(JMinvM1, force1));
dgFloat32 extenalAcceleration = -tmpAccel.AddHorizontal();
rhs->m_deltaAccel = extenalAcceleration * forceImpulseScale;
rhs->m_coordenateAccel += extenalAcceleration * forceImpulseScale;
dgAssert(rhs->m_jointFeebackForce);
const dgFloat32 force = rhs->m_jointFeebackForce->m_force * forceImpulseScale;
rhs->m_force = isBilateral ? dgClamp(force, rhs->m_lowerBoundFrictionCoefficent, rhs->m_upperBoundFrictionCoefficent) : force;
rhs->m_maxImpact = dgFloat32(0.0f);
const dgSoaFloat& JtM0 = (dgSoaFloat&)row->m_Jt.m_jacobianM0;
const dgSoaFloat& JtM1 = (dgSoaFloat&)row->m_Jt.m_jacobianM1;
const dgSoaFloat tmpDiag((weight0 * JMinvM0 * JtM0).MulAdd(weight1, JMinvM1 * JtM1));
dgFloat32 diag = tmpDiag.AddHorizontal();
dgAssert(diag > dgFloat32(0.0f));
rhs->m_diagDamp = diag * rhs->m_stiffness;
diag *= (dgFloat32(1.0f) + rhs->m_stiffness);
//rhs->m_jinvMJt = diag;
rhs->m_invJinvMJt = dgFloat32(1.0f) / diag;
}
}
void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess)
{
hacd::HaF32 b[3];
hacd::HaF32 z[3];
hacd::HaF32 d[3];
dgMatrix& mat = *this;
dgMatrix eigenVectors (initialGuess.Transpose4X4());
mat = initialGuess * mat * eigenVectors;
b[0] = mat[0][0];
b[1] = mat[1][1];
b[2] = mat[2][2];
d[0] = mat[0][0];
d[1] = mat[1][1];
d[2] = mat[2][2];
z[0] = hacd::HaF32 (0.0f);
z[1] = hacd::HaF32 (0.0f);
z[2] = hacd::HaF32 (0.0f);
for (hacd::HaI32 i = 0; i < 50; i++) {
hacd::HaF32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);
if (sm < hacd::HaF32 (1.0e-6f)) {
HACD_ASSERT (dgAbsf((eigenVectors.m_front % eigenVectors.m_front) - hacd::HaF32(1.0f)) < dgEPSILON);
HACD_ASSERT (dgAbsf((eigenVectors.m_up % eigenVectors.m_up) - hacd::HaF32(1.0f)) < dgEPSILON);
HACD_ASSERT (dgAbsf((eigenVectors.m_right % eigenVectors.m_right) - hacd::HaF32(1.0f)) < dgEPSILON);
// order the eigenvalue vectors
dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
if (tmp % eigenVectors.m_right < hacd::HaF32(0.0f)) {
eigenVectors.m_right = eigenVectors.m_right.Scale (-hacd::HaF32(1.0f));
}
eigenValues = dgVector (d[0], d[1], d[2], hacd::HaF32 (0.0f));
*this = eigenVectors.Inverse();
return;
}
hacd::HaF32 thresh = hacd::HaF32 (0.0f);
if (i < 3) {
thresh = (hacd::HaF32)(0.2f / 9.0f) * sm;
}
for (hacd::HaI32 ip = 0; ip < 2; ip ++) {
for (hacd::HaI32 iq = ip + 1; iq < 3; iq ++) {
hacd::HaF32 g = hacd::HaF32 (100.0f) * dgAbsf(mat[ip][iq]);
//if ((i > 3) && (dgAbsf(d[0]) + g == dgAbsf(d[ip])) && (dgAbsf(d[1]) + g == dgAbsf(d[1]))) {
if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
mat[ip][iq] = hacd::HaF32 (0.0f);
} else if (dgAbsf(mat[ip][iq]) > thresh) {
hacd::HaF32 t;
hacd::HaF32 h = d[iq] - d[ip];
if (dgAbsf(h) + g == dgAbsf(h)) {
t = mat[ip][iq] / h;
} else {
hacd::HaF32 theta = hacd::HaF32 (0.5f) * h / mat[ip][iq];
t = hacd::HaF32(1.0f) / (dgAbsf(theta) + dgSqrt(hacd::HaF32(1.0f) + theta * theta));
if (theta < hacd::HaF32 (0.0f)) {
t = -t;
}
}
hacd::HaF32 c = hacd::HaF32(1.0f) / dgSqrt (hacd::HaF32 (1.0f) + t * t);
hacd::HaF32 s = t * c;
hacd::HaF32 tau = s / (hacd::HaF32(1.0f) + c);
h = t * mat[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
mat[ip][iq] = hacd::HaF32(0.0f);
for (hacd::HaI32 j = 0; j <= ip - 1; j ++) {
//ROT (mat, j, ip, j, iq, s, tau);
//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau)
hacd::HaF32 g = mat[j][ip];
hacd::HaF32 h = mat[j][iq];
mat[j][ip] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (hacd::HaI32 j = ip + 1; j <= iq - 1; j ++) {
//ROT (mat, ip, j, j, iq, s, tau);
//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau)
hacd::HaF32 g = mat[ip][j];
hacd::HaF32 h = mat[j][iq];
mat[ip][j] = g - s * (h + g * tau);
mat[j][iq] = h + s * (g - h * tau);
}
for (hacd::HaI32 j = iq + 1; j < 3; j ++) {
//ROT (mat, ip, j, iq, j, s, tau);
//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau)
hacd::HaF32 g = mat[ip][j];
hacd::HaF32 h = mat[iq][j];
mat[ip][j] = g - s * (h + g * tau);
mat[iq][j] = h + s * (g - h * tau);
}
for (hacd::HaI32 j = 0; j < 3; j ++) {
//ROT (eigenVectors, j, ip, j, iq, s, tau);
//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau)
hacd::HaF32 g = eigenVectors[j][ip];
hacd::HaF32 h = eigenVectors[j][iq];
eigenVectors[j][ip] = g - s * (h + g * tau);
eigenVectors[j][iq] = h + s * (g - h * tau);
}
}
}
}
b[0] += z[0]; d[0] = b[0]; z[0] = hacd::HaF32 (0.0f);
b[1] += z[1]; d[1] = b[1]; z[1] = hacd::HaF32 (0.0f);
b[2] += z[2]; d[2] = b[2]; z[2] = hacd::HaF32 (0.0f);
}
eigenValues = dgVector (d[0], d[1], d[2], hacd::HaF32 (0.0f));
*this = dgGetIdentityMatrix();
}
void dgMatrix::PolarDecomposition (dgMatrix& transformMatrix, dgVector& scale, dgMatrix& stretchAxis, const dgMatrix* const initialStretchAxis) const
{
// a polar decomposition decompose matrix A = O * S
// where S = sqrt (transpose (L) * L)
/*
// calculate transpose (L) * L
dgMatrix LL ((*this) * Transpose());
// check is this is a pure uniformScale * rotation * translation
dgFloat32 det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * dgFloat32 (1.0f / 3.0f);
dgFloat32 invdet2 = 1.0f / det2;
dgMatrix pureRotation (LL);
pureRotation[0] = pureRotation[0].Scale3 (invdet2);
pureRotation[1] = pureRotation[1].Scale3 (invdet2);
pureRotation[2] = pureRotation[2].Scale3 (invdet2);
dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
dgFloat32 det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
if (dgAbsf (det - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f)) {
// this is a pure scale * rotation * translation
det = sign * dgSqrt (det2);
scale[0] = det;
scale[1] = det;
scale[2] = det;
det = dgFloat32 (1.0f)/ det;
transformMatrix.m_front = m_front.Scale3 (det);
transformMatrix.m_up = m_up.Scale3 (det);
transformMatrix.m_right = m_right.Scale3 (det);
transformMatrix[0][3] = dgFloat32 (0.0f);
transformMatrix[1][3] = dgFloat32 (0.0f);
transformMatrix[2][3] = dgFloat32 (0.0f);
transformMatrix.m_posit = m_posit;
stretchAxis = dgGetIdentityMatrix();
} else {
stretchAxis = LL;
stretchAxis.EigenVectors (scale);
// I need to deal with by seeing of some of the Scale are duplicated
// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
// for now just us the matrix
scale[0] = sign * dgSqrt (scale[0]);
scale[1] = sign * dgSqrt (scale[1]);
scale[2] = sign * dgSqrt (scale[2]);
scale[3] = dgFloat32 (0.0f);
dgMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]);
scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]);
scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]);
scaledAxis[3] = stretchAxis[3];
dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
transformMatrix = symetricInv * (*this);
transformMatrix.m_posit = m_posit;
}
*/
// test the f*****g factorization
dgMatrix xxxxx(dgRollMatrix(30.0f * 3.1416f / 180.0f));
xxxxx = dgYawMatrix(30.0f * 3.1416f / 180.0f) * xxxxx;
dgMatrix xxxxx1(dgGetIdentityMatrix());
xxxxx1[0][0] = 2.0f;
dgMatrix xxxxx2(xxxxx.Inverse() * xxxxx1 * xxxxx);
dgMatrix xxxxx3 (xxxxx2);
xxxxx2.EigenVectors(scale);
dgMatrix xxxxx4(xxxxx2.Inverse() * xxxxx1 * xxxxx2);
//dgFloat32 sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
dgFloat32 sign = dgSign(((*this)[0] * (*this)[1]) % (*this)[2]);
stretchAxis = (*this) * Transpose();
stretchAxis.EigenVectors (scale);
// I need to deal with by seeing of some of the Scale are duplicated
// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
// for now just us the matrix
scale[0] = sign * dgSqrt (scale[0]);
scale[1] = sign * dgSqrt (scale[1]);
scale[2] = sign * dgSqrt (scale[2]);
scale[3] = dgFloat32 (0.0f);
dgMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale3 (dgFloat32 (1.0f) / scale[0]);
scaledAxis[1] = stretchAxis[1].Scale3 (dgFloat32 (1.0f) / scale[1]);
scaledAxis[2] = stretchAxis[2].Scale3 (dgFloat32 (1.0f) / scale[2]);
scaledAxis[3] = stretchAxis[3];
dgMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
transformMatrix = symetricInv * (*this);
transformMatrix.m_posit = m_posit;
}