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); }