//Extra Methods
float Angle(const Vector3& lhs, const Vector3& rhs)
{
return (float)acos((DotProd(lhs, rhs)) / (lhs.Modulus() * rhs.Modulus()));
}
float Vector3::Angle(const Vector3& a_v3d) const
{
return (float)acos((DotProd(Vector3(x, y, z), a_v3d)) / (Vector3(x, y, z).Modulus() * a_v3d.Modulus()));
}
UInt32 RigidBodyShape::ComputeFitTo(RigidTransform3& T, const RigidBodyShape& base) const
{
size_t num_points = NumMarkers();
if (num_points == base.NumMarkers())
{
// get centroids
Vector3 centroidA(0,0,0);
Vector3 centroidB(0,0,0);
const RigidBodyMarker* iter_A = &base.marker[0];
const RigidBodyMarker* iter_B = &marker[0];
size_t ipoint(0);
size_t num_in_common(0);
for (ipoint = 0; ipoint < num_points; ipoint++, iter_A++,iter_B++)
{
if (iter_A->visible && iter_B->visible)
{
Vector3::Add(centroidA.x, centroidA.x, iter_A->position);
Vector3::Add(centroidB.x, centroidB.x, iter_B->position);
++num_in_common;
}
}
if (num_in_common >= 3)
{
// normalise
centroidA /= num_in_common;
centroidB /= num_in_common;
// correlation matrix
double sum_correl[9] =
{ 0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0
};
// find correlation matrix
iter_A = &base.marker[0];
iter_B = &marker[0];
for (ipoint = 0; ipoint < num_points; ipoint++, iter_A++, iter_B++)
{
if (iter_A->visible && iter_B->visible)
{
// input minus centroid
Vector3 normA;
Vector3::Sub(normA, iter_A->position, centroidA);
// cal minus centroid
Vector3 normB;
Vector3::Sub(normB, iter_B->position, centroidB);
// correlation (outer product)
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
sum_correl[3*i+j] += normA[i]*normB[j];
}
}
// SVD of correlation to get rotation
// from mean coords to this frame
double s[3];
double U[9], VT[9];
Matrix3x3::SVD(U, s, VT, sum_correl);
// force right-handed coord system
double detU = Matrix3x3::Det(U);
double detV = Matrix3x3::Det(VT);
if (detU*detV < 0.0)
{
VT[6] *= -1.0;
VT[7] *= -1.0;
VT[8] *= -1.0;
}
// this is the correct way round
Matrix3x3::Mul(T.R, U, VT);
// do post-subtraction of centroid B to get translation vector
double RcentroidB[3];
Matrix3x3::MulVec(RcentroidB, T.R, centroidB);
Vector3::Sub(T.t, centroidA, RcentroidB);
#if 0
iter_A = ptA;
iter_B = ptB;
for (ipoint = 0; ipoint < npoints; ipoint++, iter_A+=3, iter_B+=3)
{
Vector3 Tb;
RigidTransform3::MulVec(Tb, R, t, iter_B);
cerr << "x: " << iter_A[0] << " " << Tb[0] << endl;
cerr << "y: " << iter_A[1] << " " << Tb[1] << endl;
cerr << "z: " << iter_A[2] << " " << Tb[2] << endl;
Vector3::Sub(Tb, Tb, iter_A);
cerr << "Mod: " << Tb.Modulus() << endl;
}
#endif
return RigidBodyResult::success;
}
}
return RigidBodyResult::insufficient_points;
}
