static void Statistics (dgSphere &sphere, dgVector &eigenValues, dgVector &scaleVector, const hacd::HaF32 vertex[], hacd::HaI32 vertexCount, hacd::HaI32 stride)
{
dgBigVector var (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
dgBigVector cov (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
dgBigVector massCenter (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
const hacd::HaF32* ptr = vertex;
for (hacd::HaI32 i = 0; i < vertexCount; i ++) {
hacd::HaF32 x = ptr[0] * scaleVector.m_x;
hacd::HaF32 y = ptr[1] * scaleVector.m_y;
hacd::HaF32 z = ptr[2] * scaleVector.m_z;
ptr += stride;
massCenter += dgBigVector (x, y, z, hacd::HaF32 (0.0f));
var += dgBigVector (x * x, y * y, z * z, hacd::HaF32 (0.0f));
cov += dgBigVector (x * y, x * z, y * z, hacd::HaF32 (0.0f));
}
hacd::HaF64 k = hacd::HaF64 (1.0) / vertexCount;
var = var.Scale (k);
cov = cov.Scale (k);
massCenter = massCenter.Scale (k);
hacd::HaF64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
hacd::HaF64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
hacd::HaF64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
hacd::HaF64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
hacd::HaF64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
hacd::HaF64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector (hacd::HaF32(Ixx), hacd::HaF32(Ixy), hacd::HaF32(Ixz), hacd::HaF32 (0.0f));
sphere.m_up = dgVector (hacd::HaF32(Ixy), hacd::HaF32(Iyy), hacd::HaF32(Iyz), hacd::HaF32 (0.0f));
sphere.m_right = dgVector (hacd::HaF32(Ixz), hacd::HaF32(Iyz), hacd::HaF32(Izz), hacd::HaF32 (0.0f));
sphere.EigenVectors (eigenValues);
}
static void Statistics (
dgSphere &sphere,
dgVector &eigenValues,
dgVector &scaleVector,
const dgFloat32 vertex[],
dgInt32 vertexCount,
dgInt32 stride)
{
dgInt32 i;
const dgFloat32 *ptr;
dgFloat32 x;
dgFloat32 z;
dgFloat32 y;
dgFloat64 k;
dgFloat64 Ixx;
dgFloat64 Iyy;
dgFloat64 Izz;
dgFloat64 Ixy;
dgFloat64 Ixz;
dgFloat64 Iyz;
dgBigVector var (0.0f, 0.0f, 0.0f, 0.0f);
dgBigVector cov (0.0f, 0.0f, 0.0f, 0.0f);
dgBigVector massCenter (0.0f, 0.0f, 0.0f, 0.0f);
ptr = vertex;
for (i = 0; i < vertexCount; i ++) {
x = ptr[0] * scaleVector.m_x;
y = ptr[1] * scaleVector.m_y;
z = ptr[2] * scaleVector.m_z;
ptr += stride;
massCenter += dgBigVector (x, y, z, 0.0f);
var += dgBigVector (x * x, y * y, z * z, 0.0f);
cov += dgBigVector (x * y, x * z, y * z, 0.0f);
}
k = 1.0 / vertexCount;
var = var.Scale (k);
cov = cov.Scale (k);
massCenter = massCenter.Scale (k);
Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
Izz = var.m_z - massCenter.m_z * massCenter.m_z;
Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector (dgFloat32(Ixx), dgFloat32(Ixy), dgFloat32(Ixz), dgFloat32 (0.0f));
sphere.m_up = dgVector (dgFloat32(Ixy), dgFloat32(Iyy), dgFloat32(Iyz), dgFloat32 (0.0f));
sphere.m_right = dgVector (dgFloat32(Ixz), dgFloat32(Iyz), dgFloat32(Izz), dgFloat32 (0.0f));
sphere.EigenVectors (eigenValues);
}
static void Statistics (dgSphere &sphere, dgVector &eigenValues, dgVector &scaleVector, const hacd::HaF32 vertex[], const hacd::HaI32 faceIndex[], hacd::HaI32 indexCount, hacd::HaI32 stride)
{
dgVector var (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
dgVector cov (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
dgVector centre (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
dgVector massCenter (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
hacd::HaF64 totalArea = hacd::HaF32 (0.0f);
const hacd::HaF32* const ptr = vertex;
for (hacd::HaI32 i = 0; i < indexCount; i += 3) {
hacd::HaI32 index = faceIndex[i] * stride;
dgVector p0 (&ptr[index]);
p0 = p0.CompProduct (scaleVector);
index = faceIndex[i + 1] * stride;;
dgVector p1 (&ptr[index]);
p1 = p1.CompProduct (scaleVector);
index = faceIndex[i + 2] * stride;;
dgVector p2 (&ptr[index]);
p2 = p2.CompProduct (scaleVector);
dgVector normal ((p1 - p0) * (p2 - p0));
hacd::HaF64 area = hacd::HaF32 (0.5f) * sqrt (normal % normal);
centre = p0 + p1 + p2;
centre = centre.Scale (hacd::HaF32 (1.0f / 3.0f));
// Inertia of each point in the triangle
hacd::HaF64 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;
hacd::HaF64 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;
hacd::HaF64 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;
hacd::HaF64 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;
hacd::HaF64 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;
hacd::HaF64 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;
if (area > dgEPSILON * 10.0) {
hacd::HaF64 K = area / hacd::HaF64 (12.0);
//Coriolis theorem for Inertia of a triangle in an arbitrary orientation
Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
centre = centre.Scale ((hacd::HaF32)area);
}
totalArea += area;
massCenter += centre;
var += dgVector ((hacd::HaF32)Ixx, (hacd::HaF32)Iyy, (hacd::HaF32)Izz, hacd::HaF32 (0.0f));
cov += dgVector ((hacd::HaF32)Ixy, (hacd::HaF32)Ixz, (hacd::HaF32)Iyz, hacd::HaF32 (0.0f));
}
if (totalArea > dgEPSILON * 10.0) {
hacd::HaF64 K = hacd::HaF64 (1.0) / totalArea;
var = var.Scale ((hacd::HaF32)K);
cov = cov.Scale ((hacd::HaF32)K);
massCenter = massCenter.Scale ((hacd::HaF32)K);
}
hacd::HaF64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
hacd::HaF64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
hacd::HaF64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
hacd::HaF64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
hacd::HaF64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
hacd::HaF64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector ((hacd::HaF32)Ixx, (hacd::HaF32)Ixy, (hacd::HaF32)Ixz, hacd::HaF32 (0.0f));
sphere.m_up = dgVector ((hacd::HaF32)Ixy, (hacd::HaF32)Iyy, (hacd::HaF32)Iyz, hacd::HaF32 (0.0f));
sphere.m_right = dgVector ((hacd::HaF32)Ixz, (hacd::HaF32)Iyz, (hacd::HaF32)Izz, hacd::HaF32 (0.0f));
sphere.EigenVectors(eigenValues);
}
static void Statistics (
dgSphere &sphere,
dgVector &eigenValues,
dgVector &scaleVector,
const dgFloat32 vertex[],
const dgInt32 faceIndex[],
dgInt32 indexCount,
dgInt32 stride)
{
/*
dgInt32 i;
dgInt32 j;
dgFloat32 *ptr;
dgFloat32 x;
dgFloat32 z;
dgFloat32 y;
dgFloat64 k;
dgFloat64 Ixx;
dgFloat64 Iyy;
dgFloat64 Izz;
dgFloat64 Ixy;
dgFloat64 Ixz;
dgFloat64 Iyz;
dgBigVector massCenter (0, 0, 0, 0);
dgBigVector var (0, 0, 0, 0);
dgBigVector cov (0, 0, 0, 0);
ptr = (dgFloat32*)vertex;
for (i = 0; i < indexCount; i ++) {
j = index[i] * stride;
x = ptr[j + 0] * scaleVector.m_x;
y = ptr[j + 1] * scaleVector.m_y;
z = ptr[j + 2] * scaleVector.m_z;
massCenter += dgBigVector (x, y, z, 0);
var += dgBigVector (x * x, y * y, z * z, 0);
cov += dgBigVector (x * y, x * z, y * z, 0);
}
k = 1.0 / indexCount;
var = var.Scale (k);
cov = cov.Scale (k);
massCenter = massCenter.Scale (k);
Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
Izz = var.m_z - massCenter.m_z * massCenter.m_z;
Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector (dgFloat32(Ixx), dgFloat32(Ixy), dgFloat32(Ixz), dgFloat32 (0.0f));
sphere.m_up = dgVector (dgFloat32(Ixy), dgFloat32(Iyy), dgFloat32(Iyz), dgFloat32 (0.0f));
sphere.m_right = dgVector (dgFloat32(Ixz), dgFloat32(Iyz), dgFloat32(Izz), dgFloat32 (0.0f));
sphere.EigenVectors (eigenValues);
*/
const dgFloat32 *ptr;
dgFloat64 K;
dgFloat64 Ixx;
dgFloat64 Iyy;
dgFloat64 Izz;
dgFloat64 Ixy;
dgFloat64 Ixz;
dgFloat64 Iyz;
dgFloat64 area;
dgFloat64 totalArea;
// const dgFace *Face;
dgVector var (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector cov (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector centre (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector massCenter (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
totalArea = dgFloat32 (0.0f);
ptr = vertex;
for (dgInt32 i = 0; i < indexCount; i += 3) {
// Face = &face[i];
dgInt32 index;
index = faceIndex[i] * stride;
dgVector p0 (&ptr[index]);
p0 = p0.CompProduct (scaleVector);
index = faceIndex[i + 1] * stride;;
dgVector p1 (&ptr[index]);
p1 = p1.CompProduct (scaleVector);
index = faceIndex[i + 2] * stride;;
dgVector p2 (&ptr[index]);
p2 = p2.CompProduct (scaleVector);
dgVector normal ((p1 - p0) * (p2 - p0));
area = dgFloat32 (0.5f) * sqrt (normal % normal);
centre = p0 + p1 + p2;
centre = centre.Scale (dgFloat32 (1.0f / 3.0f));
// Inertia of each point in the triangle
Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;
Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;
Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;
Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;
Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;
Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;
if (area > dgEPSILON * 10.0) {
K = area / 12.0;
//Coriollis teorem for Inercia of a triangle in an arbitrary orientation
Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x);
Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y);
Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z);
Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y);
Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z);
Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z);
centre = centre.Scale ((dgFloat32)area);
}
totalArea += area;
massCenter += centre;
var += dgVector ((dgFloat32)Ixx, (dgFloat32)Iyy, (dgFloat32)Izz, dgFloat32 (0.0f));
cov += dgVector ((dgFloat32)Ixy, (dgFloat32)Ixz, (dgFloat32)Iyz, dgFloat32 (0.0f));
}
if (totalArea > dgEPSILON * 10.0) {
K = 1.0 / totalArea;
var = var.Scale ((dgFloat32)K);
cov = cov.Scale ((dgFloat32)K);
massCenter = massCenter.Scale ((dgFloat32)K);
}
Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
Izz = var.m_z - massCenter.m_z * massCenter.m_z;
Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
sphere.m_front = dgVector ((dgFloat32)Ixx, (dgFloat32)Ixy, (dgFloat32)Ixz, dgFloat32 (0.0f));
sphere.m_up = dgVector ((dgFloat32)Ixy, (dgFloat32)Iyy, (dgFloat32)Iyz, dgFloat32 (0.0f));
sphere.m_right = dgVector ((dgFloat32)Ixz, (dgFloat32)Iyz, (dgFloat32)Izz, dgFloat32 (0.0f));
sphere.EigenVectors(eigenValues);
}
