1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49
|
double* lineIntersect(double x_a,double y_a,double z_a,double x_b,double y_b,double z_b){//Find the point where a line intersects a plane. Also returns t, which can tell whether the point is between the two given points, or not.
/*The old method.
double t = (-1*d - (x_a*normal[0] + y_a*normal[1] + z_a*normal[2]))/((x_b-x_a)*normal[0] + (y_b-y_a)*normal[1] + (z_b-z_a)*normal[2]);
*/
double x_a0 = x_a - p0[0];
double y_a0 = y_a - p0[1];
double z_a0 = z_a - p0[2];
double original[4][4];
original[1][1] = x_a-x_b;
original[1][2] = vec1[0];
original[1][3] = vec2[0];
original[2][1] = y_a-y_b;
original[2][2] = vec1[1];
original[2][3] = vec2[1];
original[3][1] = z_a-z_b;
original[3][2] = vec1[2];
original[3][3] = vec2[2];
double inverseOfDet = 1/(threeDet(original[1][1],original[1][2],original[1][3],original[2][1],original[2][2],original[2][3],original[3][1],original[3][2],original[3][3])); //We need 1/|A|.
//cout << "invDet: " << inverseOfDet << endl;
//Here we simulataneously find the inverse and multiply it by a vector. Look at "line-plane intersection" on wiki for more info.
double t = x_a0*twoDet(original[2][2],original[2][3],original[3][2],original[3][3]) + y_a0*twoDet(original[1][3],original[1][2],original[3][3],original[3][2])+ z_a0*twoDet(original[1][2],original[1][3],original[2][2],original[2][3]);
double u = x_a0*twoDet(original[2][3],original[2][1],original[3][3],original[3][1]) + y_a0*twoDet(original[1][1],original[1][3],original[3][1],original[3][3])+ z_a0*twoDet(original[1][3],original[1][1],original[2][3],original[2][1]);
double v = x_a0*twoDet(original[2][1],original[2][2],original[3][1],original[3][2]) + y_a0*twoDet(original[1][2],original[1][1],original[3][2],original[3][1])+ z_a0*twoDet(original[1][1],original[1][2],original[2][1],original[2][2]);
t *= inverseOfDet; //Add the 1/|A| term in.
u *= inverseOfDet;
v *= inverseOfDet;
double x_isect = x_a + (x_b-x_a)*t; //Find where the line actually intersects the plane.
double y_isect = y_a + (y_b-y_a)*t;
double z_isect = z_a + (z_b-z_a)*t;
double * intersect;
try{
intersect = new double[6];
}
catch(bad_alloc){
cout << endl << "Error when allocating memory for lineIntersect in Plane.h" << endl;
}
intersect[0] = x_isect;
intersect[1] = y_isect;
intersect[2] = z_isect;
intersect[3] = t;
intersect[4] = u;
intersect[5] = v;
return intersect;
}//lineintersect
|