// Algorithm:
// plane equation: P*N + c = 0
// we find point rays in 3D from image points and camera parameters
// then we fit c by minimizing average L2 distance between rotated and translated object points
// and points found by crossing point rays with plane. We use the fact that center of mass
// of object points and fitted points should coincide.
void findPlanarObjectPose(const vector<Point3f>& _object_points, const Mat& image_points, const Point3f& normal,
const Mat& intrinsic_matrix, const Mat& distortion_coeffs, double& alpha, double& C, vector<Point3f>& object_points_crf)
{
vector<Point2f> _rays;
undistortPoints(image_points, _rays, intrinsic_matrix, distortion_coeffs);
// filter out rays that are parallel to the plane
vector<Point3f> rays;
vector<Point3f> object_points;
for(size_t i = 0; i < _rays.size(); i++)
{
Point3f ray(_rays[i].x, _rays[i].y, 1.0f);
double proj = ray.dot(normal);
if(fabs(proj) > std::numeric_limits<double>::epsilon())
{
rays.push_back(ray*(1.0/ray.dot(normal)));
object_points.push_back(_object_points[i]);
}
}
Point3f pc = massCenter(rays);
Point3f p0c = massCenter(object_points);
vector<Point3f> drays;
drays.resize(rays.size());
for(size_t i = 0; i < rays.size(); i++)
{
drays[i] = rays[i] - pc;
object_points[i] -= p0c;
}
double s1 = 0.0, s2 = 0.0, s3 = 0.0;
for(size_t i = 0; i < rays.size(); i++)
{
Point3f vprod = crossProduct(drays[i], object_points[i]);
s1 += vprod.dot(normal);
s2 += drays[i].dot(object_points[i]);
s3 += drays[i].dot(drays[i]);
}
alpha = atan2(s1, s2);
C = (s2*cos(alpha) + s1*sin(alpha))/s3;
// printf("alpha = %f, C = %f\n", alpha, C);
object_points_crf.resize(rays.size());
for(size_t i = 0; i < rays.size(); i++)
{
object_points_crf[i] = rays[i]*C;
}
}
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);
}
void dgSphere::SetDimensions(const dgFloat32 vertex[], dgInt32 strideInBytes,
const dgInt32 triangles[], dgInt32 indexCount, const dgMatrix *basis)
{
dgVector eigen;
dgVector scaleVector(dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f),
dgFloat32(0.0f));
if (indexCount < 3)
{
return;
}
dgInt32 stride = dgInt32(strideInBytes / sizeof(dgFloat32));
if (!basis)
{
InternalSphere::Statistics(*this, eigen, scaleVector, vertex, triangles,
indexCount, stride);
dgInt32 k = 0;
for (dgInt32 i = 0; i < 3; i++)
{
if (k >= 6)
{
break;
}
for (dgInt32 j = i + 1; j < 3; j++)
{
dgFloat32 aspect = InternalSphere::AspectRatio(eigen[i], eigen[j]);
if (aspect > dgFloat32(0.9f))
{
scaleVector[i] *= dgFloat32(2.0f);
InternalSphere::Statistics(*this, eigen, scaleVector, vertex,
triangles, indexCount, stride);
k++;
i = -1;
break;
}
}
}
}
else
{
*this = *basis;
}
dgVector min;
dgVector max;
InternalSphere::BoundingBox(*this, vertex, stride, triangles, indexCount, min,
max);
dgVector massCenter(max + min);
massCenter = massCenter.Scale(dgFloat32(0.5f));
m_posit = TransformVector(massCenter);
dgVector dim(max - min);
dim = dim.Scale(dgFloat32(0.5f));
SetDimensions(dim.m_x, dim.m_y, dim.m_z);
}
/*!
* \brief buildMassCenters Use this function to retrieve the mass centers of _contours.
* \param _contours Input contours.
* \param _massCenters Output mass centers.
*/
void ContourManager::buildMassCenters(const ContourVector & _contours, PointVector & _massCenters)
{
ContourVector::size_type contourSize = _contours.size();
_massCenters.resize(contourSize);
for (ContourVector::size_type i = 0; i < contourSize; ++i)
{
massCenter(_contours[i],_massCenters[i]);
}//for(ContourVector::size_type i = 0; i < contourSize; ++i)
}//buildMassCenters
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);
}
void dgSphere::SetDimensions (
const dgFloat32 vertex[],
dgInt32 strideInBytes,
dgInt32 count,
const dgMatrix *basis)
{
dgInt32 i;
dgInt32 j;
dgInt32 k;
dgInt32 stride;
dgFloat32 aspect;
dgVector eigen;
dgVector scaleVector (1.0f, 1.0f, 1.0f, 0.0f);
stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
if (!basis) {
InternalSphere::Statistics (*this, eigen, scaleVector, vertex, count, stride);
k = 0;
for (i = 0; i < 3; i ++) {
if (k >= 6) {
break;
}
for (j = i + 1; j < 3; j ++) {
aspect = InternalSphere::AspectRatio (eigen[i], eigen[j]);
if (aspect > 0.9) {
scaleVector[i] *= 2.0f;
InternalSphere::Statistics (*this, eigen, scaleVector, vertex, count, stride);
k ++;
i = -1;
break;
}
}
}
} else {
*this = *basis;
}
dgVector min;
dgVector max;
InternalSphere::BoundingBox (*this, vertex, count, stride, min, max);
dgVector massCenter (max + min);
massCenter = massCenter.Scale (0.5);
m_posit = TransformVector (massCenter);
dgVector dim (max - min);
dim = dim.Scale (0.5f);
SetDimensions (dim.m_x + InternalSphere::SPHERE_TOL,
dim.m_y + InternalSphere::SPHERE_TOL,
dim.m_z + InternalSphere::SPHERE_TOL);
}
void fit2DRotation(const vector<Point3f>& points1, const vector<Point3f>& points2, Point3f normal, Mat& rvec)
{
Point3f center1 = massCenter(points1);
Point3f center2 = massCenter(points2);
float sum1 = 0.0f, sum2 = 0.0f;
for(size_t i = 0; i < points1.size(); i++)
{
Point3f off1 = points1[i] - center1;
Point3f off2 = points2[i] - center2;
Point3f off1p = crossProduct(off1, normal);
sum1 += off2.dot(off1);
sum2 += off2.dot(off1p);
}
double alpha = -atan2(sum1, sum2);
if(rvec.empty() == true)
{
rvec.create(3, 1, CV_32F);
}
rvec.at<Point3f>(0, 0) = normal*(alpha/norm(normal));
}
void ShapeMatching<PFP>::initialize()
{
Eigen::Vector3d x0cm = massCenter();
for(unsigned int i = m_position.begin() ; i < m_position.end() ; m_position.next(i))
{
Eigen::Vector3d tmp ;
for (unsigned int j = 0 ; j < 3 ; ++j)
tmp(j) = m_position[i][j] ;
Eigen::Vector3d qi = tmp - x0cm;
m_q.push_back(qi); //q_{i} = x^{0}_{i} - x^{0}_{cm}
}
}
bool closeSurface( fwVertexPosition &_vertex, fwVertexIndex &_vertexIndex )
{
typedef std::pair< int, int > Edge;
typedef std::vector< Edge > Contour; // at Border
typedef std::vector< Contour> Contours;
Contours contours;
findBorderEdges( _vertexIndex , contours);
bool closurePerformed = !contours.empty() ;
// close each hole
for ( Contours::iterator contour=contours.begin(); contour != contours.end(); ++contour )
{
int newVertexIndex = _vertex.size() ;
// create gravity point & insert new triangle
std::vector< float > massCenter(3,0);
for ( Contour::iterator edge =contour->begin(); edge != contour->end(); ++edge )
{
for (int i=0; i<3; ++i )
{
massCenter[i] += _vertex[edge->first][i];
massCenter[i] += _vertex[edge->second][i];
}
// create new Triangle
std::vector< int > triangleIndex(3);
triangleIndex[0] = edge->first;
triangleIndex[1] = edge->second;
triangleIndex[2] = newVertexIndex;
_vertexIndex.push_back( triangleIndex ); // TEST
}
for (int i=0; i<3; ++i )
{
massCenter[i] /= contour->size()*2;
}
_vertex.push_back( massCenter ); // normalize barycenter
}
return closurePerformed;
}
void solvePlanarPnP(const Mat& objectPoints, const Mat& imagePoints, const Mat& cameraMatrix, const Mat& distCoeffs,
Mat& _rvec, Mat& _tvec, bool useExtrinsicGuess)
{
CV_Assert(objectPoints.depth() == CV_32F && imagePoints.depth() == CV_32F);
if(useExtrinsicGuess == false)
{
solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, _rvec, _tvec, false);
return;
}
Mat rvec, tvec;
_rvec.convertTo(rvec, CV_32FC1);
_tvec.convertTo(tvec, CV_32FC1);
// calculate rotation matrix
Mat R(3, 3, CV_32FC1);
Rodrigues(rvec, R);
CV_Assert(R.type() == CV_32FC1);
// calculate object normal
Point3f normal0 = getPlanarObjectNormal(objectPoints);
// printf("Normal0: %f %f %f\n", normal0.x, normal0.y, normal0.z);
Mat Normal0(3, 1, CV_32F);
Normal0.at<Point3f>(0, 0) = normal0;
Mat Normal = R*Normal0;
Point3f normal = Normal.at<Point3f>(0, 0);
normal = normal*(1.0/norm(normal));
if(normal.z < 0) normal = -normal; // z points from the camera
// printf("Normal: %f %f %f\n", normal.x, normal.y, normal.z);
vector<Point3f> rotated_object_points;
rotated_object_points.resize(objectPoints.rows);
for(size_t i = 0; i < rotated_object_points.size(); i++)
{
Mat p = objectPoints.rowRange(i, i + 1);
p = p.reshape(1, 3);
Mat res = R*p;
rotated_object_points[i] = res.at<Point3f>(0, 0);
}
double alpha, C;
vector<Point3f> object_points_crf;
findPlanarObjectPose(rotated_object_points, imagePoints, normal, cameraMatrix, distCoeffs, alpha, C, object_points_crf);
Mat rp(3, 1, CV_32FC1);
rp.at<Point3f>(0, 0) = normal*alpha;
Mat Rp;
Rodrigues(rp, Rp);
R = Rp*R;
Rodrigues(R, rvec);
Point3f center1 = massCenter(rotated_object_points);
Mat mcenter1(3, 1, CV_32FC1, ¢er1);
Mat mcenter1_alpha = Rp*mcenter1;
Point3f center1_alpha = mcenter1_alpha.at<Point3f>(0, 0);
Point3f center2 = massCenter(object_points_crf);
tvec.at<Point3f>(0, 0) = center2 - center1_alpha;
Mat mobj;
objectPoints.copyTo(mobj);
mobj = mobj.reshape(1);
CV_Assert(R.type() == CV_32FC1 && mobj.type() == CV_32FC1);
Mat mrobj = R*mobj.t();
mrobj = mrobj.t();
Point3f p1 = mrobj.at<Point3f>(0, 0) + center2 - center1;
Point3f p2 = object_points_crf[0];
// printf("point1: %f %f %f\n", p1.x, p1.y, p1.z);
// printf("point2: %f %f %f\n", p2.x, p2.y, p2.z);
rvec.convertTo(_rvec, _rvec.depth());
tvec.convertTo(_tvec, _tvec.depth());
}
static void Statistics (dgObb &sphere, dgVector &eigenValues, const dgVector &scale, const dgFloat32 vertex[], const dgInt32 faceIndex[], dgInt32 indexCount, dgInt32 stride)
{
dgVector var (dgFloat32 (0.0f));
dgVector cov (dgFloat32 (0.0f));
dgVector centre (dgFloat32 (0.0f));
dgVector massCenter (dgFloat32 (0.0f));
dgVector scaleVector (scale & dgVector::m_triplexMask);
dgFloat64 totalArea = dgFloat32 (0.0f);
const dgFloat32* const ptr = vertex;
for (dgInt32 i = 0; i < indexCount; i += 3) {
dgInt32 index = faceIndex[i] * stride;
dgVector p0 (&ptr[index]);
p0 = p0 * scaleVector;
index = faceIndex[i + 1] * stride;;
dgVector p1 (&ptr[index]);
p1 = p1 * scaleVector;
index = faceIndex[i + 2] * stride;;
dgVector p2 (&ptr[index]);
p2 = p2 * scaleVector;
dgVector normal ((p1 - p0).CrossProduct(p2 - p0));
dgFloat64 area = dgFloat32 (0.5f) * sqrt (normal.DotProduct3(normal));
centre = p0 + p1 + p2;
centre = centre.Scale (dgFloat32 (1.0f / 3.0f));
// Inertia of each point in the triangle
dgFloat64 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x;
dgFloat64 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y;
dgFloat64 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z;
dgFloat64 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y;
dgFloat64 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z;
dgFloat64 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z;
if (area > dgEPSILON * 10.0) {
dgFloat64 K = area / dgFloat64 (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 ((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) {
dgFloat64 K = dgFloat64 (1.0) / totalArea;
var = var.Scale ((dgFloat32)K);
cov = cov.Scale ((dgFloat32)K);
massCenter = massCenter.Scale ((dgFloat32)K);
}
dgFloat64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
dgFloat64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
dgFloat64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
dgFloat64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
dgFloat64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
dgFloat64 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);
}
void grenade::generateExplosion()
{
effect* _effect = new effect(this->GetPosition().x, this->GetPosition().y, 128,128,8,8,explTime,images::explosions::missile_explosion,sf::Vector2f(0,0));
Game::GetGameObjectManager().Add(_effect);
if(multiplayer::_peerState == multiplayer::server)
{
sf::Packet outPacket;
outPacket.clear();
networkEvent currentNetEvent;
currentNetEvent.myType = networkEvent::destroyProjectile;
currentNetEvent.projNum = this->projNum;
outPacket << currentNetEvent;
multiplayer::sendToClients(outPacket);
outPacket.clear();
sf::Vector2f explCenter = this->GetPosition();
for(std::map<int,peer*>::iterator it = multiplayer::allPeers.begin(); it != multiplayer::allPeers.end(); ++it)
{
if( !it->second->hisPlayer->getIsDead() && !it->second->hisPlayer->getIsRespawning() )
{
sf::FloatRect legRect = it->second->hisPlayer->GetBoundingRect();
sf::FloatRect bodyRect = it->second->hisPlayer->getBody().GetBoundingRect();
sf::Vector2f playerPos;
sf::Vector2f massCenter(legRect.left+legRect.width/2,legRect.top);
float left = std::min(legRect.left,bodyRect.left);
float right = std::max(legRect.left+legRect.width,bodyRect.left+bodyRect.width);
float top = std::min(legRect.top,bodyRect.top);
float bottom = std::max(legRect.top+legRect.height,bodyRect.top+bodyRect.height);
// use closest edge of player to explosion as their position
if ( explCenter.x < left )
playerPos.x = left;
else if ( explCenter.x > right )
playerPos.x = right;
else
playerPos.x = explCenter.x;
if ( explCenter.y < top )
playerPos.y = top;
else if ( explCenter.y > bottom )
playerPos.y = bottom;
else
playerPos.y = explCenter.y;
// knockback vector
sf::Vector2f knockDir(massCenter.x-explCenter.x , massCenter.y-explCenter.y);
float mag = sqrt(knockDir.x*knockDir.x+knockDir.y*knockDir.y);
knockDir.x = knockDir.x/mag;
knockDir.y = knockDir.y/mag;
knockDir.x*=knockForce;
knockDir.y*=knockForce;
float sqDistance = pow( (explCenter.x-playerPos.x), 2) + pow( (explCenter.y-playerPos.y), 2);
float sqRadius = pow( expRadius, 2);
int damageTaken = (int) floor (playerDamage*(1-sqDistance/sqRadius) );
bool shouldDamage;
bool onSameTeam;
bool isMe;
onSameTeam = (it->second->team == multiplayer::allPeers.find(this->ownerPlayerNum)->second->team);
isMe = (this->ownerPlayerNum == it->first);
shouldDamage = ( damageTaken > 0) &&
( (Game::myGameType == Game::ffa) || ( (Game::myGameType == Game::teams)&&(!onSameTeam || Game::ffIsOn || isMe) ) );
if(shouldDamage)
{
std::cout << it->second->name << " takes: " << damageTaken << std:: endl;
it->second->hisPlayer->knockBack(knockDir,knockTime);
sf::Packet outPacket;
outPacket.clear();
networkEvent knockEvent;
knockEvent.myType = networkEvent::knockBack;
knockEvent.playerNumber = it->first;
knockEvent.knockDirecion = knockDir;
knockEvent.knockTime = knockTime;
outPacket << knockEvent;
multiplayer::sendToClients(outPacket);
outPacket.clear();
it->second->hisPlayer->takeDamage(damageTaken);
outPacket.clear();
networkEvent currentNetEvent;
currentNetEvent.myType = networkEvent::playerHit;
currentNetEvent.playerNumber = it->first;
currentNetEvent.playerDamage = damageTaken;
outPacket << currentNetEvent;
multiplayer::sendToClients(outPacket);
outPacket.clear();
}
}
}
int i_left;
int j_top;
int i_right;
int j_bottom;
i_left = (int) floor( (explCenter.x-expRadius) / Game::GRID_WIDTH);
j_top = (int) floor( (explCenter.y-expRadius) / Game::GRID_HEIGHT);
i_right = (int) ceil( (explCenter.x+expRadius) / Game::GRID_WIDTH);
j_bottom = (int) ceil( (explCenter.y+expRadius) / Game::GRID_HEIGHT);
if (i_left < 0)
i_left = 0;
if (i_left >= Game::mapWidth)
i_left = Game::mapWidth-1;
if (i_right < 0)
i_right = 0;
if (i_right >= Game::mapWidth)
i_right = Game::mapWidth-1;
if (j_top < 0)
j_top = 0;
if (j_top >= Game::mapHeight)
j_top = Game::mapHeight-1;
if (j_bottom < 0)
j_bottom = 0;
if (j_bottom >= Game::mapHeight)
j_bottom = Game::mapHeight-1;
int i;
int j;
for(i = i_left; i <= i_right; i++)
{
for(j = j_top; j <= j_bottom; j++)
{
float sqDistance;
sqDistance = pow( (Game::gameGrid[i][j].GetPosition().x+Game::GRID_WIDTH/2-explCenter.x) , 2 ) +
pow( (Game::gameGrid[i][j].GetPosition().y+Game::GRID_HEIGHT/2-explCenter.y) , 2 ) ;
float sqRadius = pow( expRadius, 2);
int damageTaken = (int) floor (boxDamage*(1-sqDistance/sqRadius) );
if( damageTaken > 0 && Game::gameGrid[i][j].getIsDrawn())
{
Game::gameGrid[i][j].takeDamage(damageTaken);
sf::Packet outPacket;
outPacket.clear();
networkEvent currentNetEvent;
currentNetEvent.myType = networkEvent::boxHit;
currentNetEvent.boxX = i;
currentNetEvent.boxY = j;
currentNetEvent.boxDamage = damageTaken;
outPacket << currentNetEvent;
multiplayer::sendToClients(outPacket);
outPacket.clear();
}
}
}
}
}
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);
}
void align(float** xyzA, float** xyzB, int *zeros, int size) {
massCenter(xyzA, zeros, size);
massCenter(xyzB, zeros, size);
// PStruct
// double dist;
double u[4][4];
double omega[7][7]; // 7 to use fortran like notation in loops
double o[7][7]; // eigen vector SET BY EIGEN
double ha[4][4];
double ka[4][4];
double r[4][4]; // rotation
double op[4];
double s;
double det;
double d [6+1]; // eigen val SET BY EIGEN and not used...
// ===== Allocate rotated structure
double (*xyzn)[3] = new double [size][3];
// ===== Compute rotation matrix
int i;
for (i = 1; i <= 3; i++) {
for (int j = 1; j <= 3; j++) {
u[i][j] = 0.0;
//o[i][j] = 0.0;
ha[i][j] = 0; //
ka[i][j] = 0; //
r[i][j] = 0; //
}
d[i] = 0.;
}
// additional init PF TEST
for (i = 1; i <= 6; i++) {
for (int j = 1; j <= 6; j++) {
o[i][j] = 0.0;
// omega done in original code later
}
d[i] = 0.;
op[i] = 0.;
}
int k;
// cout << "skipping " << flush;
for (k = 0; k < size; k++) {
if (zeros[k]==0) {
// cout << k << " " << zeros[k] << " " << flush;
continue;
}
u[1][1] = u[1][1] + xyzA[k][0] * xyzB[k][0];
u[1][2] = u[1][2] + xyzA[k][0] * xyzB[k][1];
u[1][3] = u[1][3] + xyzA[k][0] * xyzB[k][2];
u[2][1] = u[2][1] + xyzA[k][1] * xyzB[k][0];
u[2][2] = u[2][2] + xyzA[k][1] * xyzB[k][1];
u[2][3] = u[2][3] + xyzA[k][1] * xyzB[k][2];
u[3][1] = u[3][1] + xyzA[k][2] * xyzB[k][0];
u[3][2] = u[3][2] + xyzA[k][2] * xyzB[k][1];
u[3][3] = u[3][3] + xyzA[k][2] * xyzB[k][2];
}
// cout << endl << flush;
det = u[1][1] * u[2][2] * u[3][3] +
u[1][2] * u[2][3] * u[3][1] +
u[1][3] * u[2][1] * u[3][2] -
u[1][3] * u[2][2] * u[3][1] -
u[1][1] * u[2][3] * u[3][2] -
u[1][2] * u[2][1] * u[3][3];
//cout << "DET= " << det << endl << flush;
for (i = 1; i <= 6; i++) {
for (int j = 1; j <= 6; j++) {
omega[i][j] = 0.0;
}
}
for (i = 1; i <= 3; i ++) {
for (int j = 4; j <= 6; j++) {
omega[i][j] = u[i][j-3];
}
}
for (i = 4; i <= 6; i++) {
for (int j = 1; j <= 3; j++) {
omega[i][j] = u[j][i-3];
}
}
// eigen sets o (eigen vector) and d (eigen val)
eigen(omega,o,d,6);
// o values set by eigen used below
// d not used
for (k=1; k<=3; k++) {
for (i = 1; i<=3; i++) {
ha[i][k] = o[i][k];
ka[i][k] = o[i + 3][k];
}
}
for (k = 1; k<=3; k++) {
s = 0.;
for (i = 1; i<=3; i++) {
s += ha[i][k] * ha[i][k] ;
}
s = sqrt(s);
for (i = 1; i<=3; i++) {
ha[i][k] = ha[i][k]/s;
}
}
for (k = 1; k <= 3; k++) {
s = 0.0;
for (i = 1; i<=3; i++) {
s += ka[i][k]* ka[i][k];
}
s = sqrt(s);
for (i = 1; i <= 3; i++) {
ka[i][k] = ka[i][k]/s;
}
}
op[1] = ha[2][1] * ha[3][2]-ha[3][1] * ha[2][2];
op[2] = ha[3][1] * ha[1][2]-ha[1][1] * ha[3][2];
op[3] = ha[1][1] * ha[2][2]-ha[2][1] * ha[1][2];
s = op[1] * ha[1][3] + op[2] * ha[2][3] + op[3] * ha[3][3];
if(s < 0.) {
for (k=1; k <= 3; k++) {
ha[k][3] = -ha[k][3];
}
}
op[1] = ka[2][1] * ka[3][2]-ka[3][1] * ka[2][2];
op[2] = ka[3][1] * ka[1][2]-ka[1][1] * ka[3][2];
op[3] = ka[1][1] * ka[2][2]-ka[2][1] * ka[1][2];
s = op[1] * ka[1][3] + op[2] * ka[2][3] + op[3] * ka[3][3];
if(det > 0.) {
if(s < 0.) {
for (k = 1; k<=3; k++) {
ka[k][3] = -ka[k][3];
}
}
}
else {
if(s > 0.0) {
for (k = 1; k<=3; k++) {
ka[k][3] = -ka[k][3];
}
}
}
k = 1;
if(det < 0.) k = -1;
// rotation matrix
for (i = 1; i<=3; i++) {
for (int j = 1; j<=3; j++) {
r[i][j] = ka[i][1] * ha[j][1] + ka[i][2] * ha[j][2] + k * ka[i][3] * ha[j][3];
}
}
for (i=0; i < size; i++) {
xyzn[i][0]=r[1][1]*xyzA[i][0]+r[1][2]*xyzA[i][1]+r[1][3]*xyzA[i][2];
xyzn[i][1]=r[2][1]*xyzA[i][0]+r[2][2]*xyzA[i][1]+r[2][3]*xyzA[i][2];
xyzn[i][2]=r[3][1]*xyzA[i][0]+r[3][2]*xyzA[i][1]+r[3][3]*xyzA[i][2];
xyzA[i][0] = xyzn[i][0];
xyzA[i][1] = xyzn[i][1];
xyzA[i][2] = xyzn[i][2];
}
if (xyzn) delete [] xyzn;
}
void ShapeMatching<PFP>::shapeMatch()
{
// p_{i}
std::vector<Eigen::Vector3d> m_p;
m_p.reserve(m_position.nbElements());
//1.
Eigen::Vector3d xcm = massCenter();
for(unsigned int i = m_position.begin() ; i < m_position.end() ; m_position.next(i))
{
Eigen::Vector3d tmp ;
for (unsigned int j = 0 ; j < 3 ; ++j)
tmp(j) = m_position[i][j] ;
Eigen::Vector3d pi = tmp - xcm ;
m_p.push_back(pi) ; //p_{i} = x_{i} - x_{cm}
}
//2.
Eigen::Matrix3d apq = Eigen::Matrix3d::Zero();
for(unsigned int i=0 ; i < m_p.size() ; ++i)
{
apq(0,0) += m_p[i][0] * m_q[i][0];
apq(0,1) += m_p[i][0] * m_q[i][1];
apq(0,2) += m_p[i][0] * m_q[i][2];
apq(1,0) += m_p[i][1] * m_q[i][0];
apq(1,1) += m_p[i][1] * m_q[i][1];
apq(1,2) += m_p[i][1] * m_q[i][2];
apq(2,0) += m_p[i][2] * m_q[i][0];
apq(2,1) += m_p[i][2] * m_q[i][1];
apq(2,2) += m_p[i][2] * m_q[i][2];
}
Eigen::Matrix3d S = apq.transpose() * apq ; //symmetric matrix
//3. Jacobi Diagonalisation
Eigen::EigenSolver<Eigen::Matrix3d> es(S);
//V * D * V^(-1)
Eigen::Matrix3d D = es.pseudoEigenvalueMatrix();
Eigen::Matrix3d U = es.pseudoEigenvectors() ;
for(int j = 0; j < 3; j++)
{
if(D(j,j) <= 0)
{
D(j,j) = 0.05f;
}
D(j,j) = 1.0f/sqrt(D(j,j));
}
S = U * D * U.transpose();
// Now we can get the rotation part
Eigen::Matrix3d R = apq * S; //S^{-1}
//4.
for(unsigned int i = m_goal.begin() ; i < m_goal.end() ; m_goal.next(i))
{
Eigen::Vector3d tmp = R * m_q[i] + xcm; // g_{i} = R * q_i + x_{cm}
VEC3 g;
for (unsigned int j = 0 ; j < 3 ; ++j)
g[j] = tmp(j);
m_goal[i] = g;
}
}
void Environment::makeReward()
{
physics::Vector3r gravity = physics::currentPhysicsManager().getDynamicsWorld()->getGravity();
// find center of mass of the chain
centerHeight = 0;
physics::real mass(0.0);
physics::Vector3r massCenter(0);
physics::Vector3r footHold( math::get_translation( rigidBodies[0]->getTransform() ) );
for (size_t i = 0; i<rigidBodies.size(); ++i)
{
physics::Vector3r center = math::get_translation( rigidBodies[i]->getTransform() );
centerHeight += center.y / rigidBodies.size();
if ( math::dot(footHold, gravity) < math::dot(center, gravity) ) {
footHold = center;
}
if ( rigidBodies[i]->getDynamicsType() == physics::RigidBody::DT_DYNAMIC )
{
massCenter += math::get_translation( rigidBodies[i]->getTransform() ) * rigidBodies[i]->getMass();
mass += rigidBodies[i]->getMass();
}
}
massCenter /= mass;
// if two or more contacts with floor, then restart sausage
/*
contactUpdateMutex.lock();
if (activeContacts.size() >= 2 || centerHeight < 0.5f * initialCenterHeight) {
terminal = true;
}
contactUpdateMutex.unlock();
*/
//reward = centerHeight - initialCenterHeight;
// calculate mass center deviation
const physics::real deviationFactor = 2.0;
reward = 0.0;
{
physics::Vector3r vec = massCenter - footHold;
if ( math::length(vec) < math::EPS_3f ) {
reward = 0.0;
}
else
{
// cos(vec, gravity) ^ deviationFactor
physics::real deviation = powf( math::dot(vec, -gravity) / ( math::length(gravity) * math::length(vec) ), deviationFactor );
reward = deviation - 1.0;
}
}
/*
if (episodic)
{
float posDeviation=0, velDeviation=0;
bool fallxhigh = true;
bool fallxlow = true;
bool fallyhigh = true;
bool fallylow = true;
for (size_t i = 0; i<constraintAngles.size(); ++i)
{
math::Vector3f lowLimit = constraints[i]->getStateDesc().angularLimits[0];
math::Vector3f highLimit = constraints[i]->getStateDesc().angularLimits[1];
posDeviation += pow(2.0f * (constraintAngles[i].x - lowLimit.x) / (highLimit.x - lowLimit.x) - 1.0f, 2);
posDeviation += pow(2.0f * (constraintAngles[i].y - lowLimit.y) / (highLimit.y - lowLimit.y) - 1.0f, 2);
velDeviation += pow(2.0f * std::min(std::max(constraintVelocities[i].x, -maxVelocity), maxVelocity) / maxVelocity - 1.0f,2);
velDeviation += pow(2.0f * std::min(std::max(constraintVelocities[i].y, -maxVelocity), maxVelocity) / maxVelocity - 1.0f,2);
fallxhigh = fallxhigh && (highLimit.x - constraintAngles[i].x < 0.01);
fallxlow = fallxlow && (constraintAngles[i].x - lowLimit.x < 0.01);
fallyhigh = fallyhigh && (highLimit.y - constraintAngles[i].y < 0.01);
fallylow = fallylow && (constraintAngles[i].y - lowLimit.y < 0.01);
//fall = fall && ((constraintAngles[i].x - lowLimit.x < 0.01) || (highLimit.x - constraintAngles[i].x < 0.01) ||
// (constraintAngles[i].y - lowLimit.y < 0.01) || (highLimit.y - constraintAngles[i].y < 0.01));
}
posDeviation /= constraintAngles.size()*2.0f;
velDeviation /= constraintAngles.size()*2.0f;
averagePos = posDeviation + 0.9f*averagePos;
averageVel = velDeviation + 0.9f*averageVel;
float forceNorm = 0;
for (size_t i = 0; i < motorForces.size(); ++i)
{
forceNorm += motorForces[i]*motorForces[i];
}
//reward -= 0.001*forceNorm;
reward -= 0.001f*velDeviation;
if (averagePos < 0.01 && averageVel < 0.01)
{
terminal = true;
reward = 1.0;
}
if (fallxhigh || fallxlow || fallyhigh || fallylow)
{
terminal = true;
reward = -6.0;
}
}
*/
{
boost::lock_guard<boost::mutex> lock(stateMutex);
state = READY;
}
rewardReadyCondition.notify_one();
}
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);
}
