Matrix * Pose::intersectLineWithXYPlane(vector<Matrix *> * aLine){
Matrix *l1 = aLine->at(0);
Matrix *l2 = aLine->at(1);
//points on the plane level with the ground in the horizon coord frame
//normally need 3 points, but since one is the origin, it can get ignored
Matrix * horizonUnitX = point4D(1,0,0);
Matrix * horizonUnitY = point4D(0,1,0);
//we now solve the point of intersection using linear algebra
//Ax=b, where b is the target, x is the solution of weights (t,u,v)
//to solve l1 + (l2 -l1)t = o1*u + o2*v
//Note!: usually a plane is defined by three vectors, but in this
//case, the horizon plane goes through the origin of the horizon
//frame, so one of the vectors is the zero vector, so we ignore it
//See http://en.wikipedia.org/wiki/Line-plane_intersection for detail
float contents[] =
{*fmget(l1,0,0) - *fmget(l2,0,0), *fmget(horizonUnitX,0,0),
*fmget(horizonUnitY,0,0),
*fmget(l1,1,0) - *fmget(l2,1,0), *fmget(horizonUnitX,1,0),
*fmget(horizonUnitY,1,0),
*fmget(l1,2,0) - *fmget(l2,2,0), *fmget(horizonUnitX,2,0),
*fmget(horizonUnitY,2,0)
};
Matrix *eqSystem = fmatrix(3,3);
memcpy(eqSystem->data,contents,3*3*sizeof(float));
// Solve for the solution of the weights.
// Now usually we would fmsolve(eqSystem,l1), but l1 is defined in
// homogeneous coordiantes. We need it to be a 3 by 1 vector to solve the
// system of equations.
Matrix *target = fmatrix4(l1,0,3,0,1);
Matrix *result = fmsolve(eqSystem,target);
float t = *fmget(result,0,0);
fmfree(result); fmfree(eqSystem);
fmfree(horizonUnitX); fmfree(horizonUnitY);
//solution.get(0,0) contains the value of the parameter t
//such that the point l1 + (l2 -l1)t is on the horizon plane
//NOTE: this intersection is still in the horizon frame though
Matrix *intersection = fmsub(l2,l1);
fmscaleeq(intersection,t);
fmaddeq(intersection,l1);
return intersection;
}
// Floating Multiply-Sub Single
static void
fmsubs(ThreadState *state, Instruction instr)
{
return fmsub(state, instr);
}
