bool VRPNTrackerAxis::parseTransform(const std::string& axis,
Matrix3f& transformOut )
{
static const boost::regex k_axis_re("^([xyzXYZ])([xyzXYZ])([xyzXYZ])$");
static std::map<char,Vector3f> k_axisMap =
boost::assign::map_list_of ('X', Vector3f::UNIT_X)
('Y', Vector3f::UNIT_Y)
('Z', Vector3f::UNIT_Z);
boost::smatch optResult;
if (boost::regex_search(axis, optResult, k_axis_re))
{
for (size_t r = 0; r < transformOut.ROWS; ++r)
{
std::string optionValue(optResult.str(r+1));
LBASSERT(optionValue.size() == 1);
const char ax = optionValue[0];
const float scale = std::isupper(ax) ? 1.f : -1.f;
const Vector3f v = scale * k_axisMap[std::toupper(ax)];
transformOut.set_row(r, v);
}
}
else
return false;
return true;
}
Vector3f computePlaneIntersection( const Plane& plane0, const Plane& plane1, const Plane& plane2 )
{
// TODO: Paralllel planes, whew !
Matrix3f linSolve;
linSolve.set_row( 0, plane0.getNormal() );
linSolve.set_row( 1, plane1.getNormal() );
linSolve.set_row( 2, plane2.getNormal() );
Matrix3f ilinSolve;
linSolve.inverse( ilinSolve );
Vector3f bSolve( -plane0.getd(),
-plane1.getd(),
-plane2.getd() );
return ilinSolve * bSolve;
}