Need an algorithm for 3D vectors intersection - vector

Question

I have 2 vectors, each defined by 2 Point3D (origin and direction). I need to find out the point of their intersection.
A little bit of help is always welcome.
I will post my function, which gives me wrong output.
public static CurvIntersect3D Intersect2Linii3D (Vector3D dr1, Vector3D dr2) {
CurvIntersect3D result = new CurvIntersect3D(0, null);
double x = Math3D.VectorNorm3D(dr1.getDirectie());
double t = Math3D.VectorNorm3D(dr2.getDirectie());
double cosa = (dr1.getDirectie().getX()*dr2.getDirectie().getX() + dr1.getDirectie().getY()*dr2.getDirectie().getY() + dr1.getDirectie().getZ()*dr2.getDirectie().getZ()) / (t*x);
Punct3D p1 = dr1.getOrigine();
Punct3D p2 = new Punct3D(), p3 = new Punct3D();
for (int i=0; i<3; i++)
{
p2.set(i, dr1.getOrigine().get(i) + dr1.getDirectie().get(i));
p3.set(i, dr1.getOrigine().get(i) + dr2.getDirectie().get(i));
}
Matrici.Matrice3x3 rot = Math3D.GetMatriceRotatie(p1, p2, p3);
Punct3D orig = new Punct3D();
for (int i=0; i<3; i++)
orig.set(i, rot.getElement(i, 0) * (dr2.getOrigine().getX()-dr1.getOrigine().getX()) +
rot.getElement(i, 1) * (dr2.getOrigine().getY()-dr1.getOrigine().getY()) +
rot.getElement(i, 2) * (dr2.getOrigine().getZ()-dr1.getOrigine().getZ()));
x = orig.getY() - orig.getZ()* cosa / Math.sqrt(1 - cosa*cosa);
p1 = new Punct3D();
for (int i=0; i<3; i++)
p1.set(i, dr1.getOrigine().get(i) + x*dr1.getDirectie().get(i));
result.setCount(1);
result.add(p1);
return result;
}
CurvIntersec3D is a structure that stores the array of points and its length.

Answer 1

As mentioned before the two lines may not meet at a single point. The best you can do in general is find the point on line1 closest to line2 and vise versa. Connect those two points to create the common normal direction.
Given two lines passing through 3D points r1=[r1x,r1y,r1z] and r2=[r2x,r2y,r2z] and having unit directions e1=[e1x,e1y,e1z] and e2=[e2x,e2y,e2z] you can find the points on the line which are closest to the other line like this:
Find the direction projection u=Dot(e1,e2)=e1x*e2x+e1y*e2y+e1z*e2z
If u==1 then lines are parallel. No intersection exists.
Find the separation projections t1=Dot(r2-r1,e1) and t2=Dot(r2-r1,e2)
Find distance along line1 d1 = (t1-u*t2)/(1-u*u)
Find distance along line2 d2 = (t2-u*t1)/(u*u-1)
Find the point on line1 p1=Add(r1,Scale(d1,e1))
Find the point on line2 p2=Add(r2,Scale(d2,e2))
Note: You must have the directions as unit vectors, Dot(e1,e1)=1 and Dot(e2,e2)=1.
The function Dot() is the vector dot product. The function Add() adds the components of vectors, and the function Scale() multiplies the components of the vector with a number.
Good luck.

Answer 2

Are you sure that your lines have intersection?
If it is guaranteed, then the problem is rather simple: Get parametric equations of lines, solve a system of two linear equations like these:
A_X0+t*A_Dir_X = B_X0+u*B_DirX , where X0 is base point, and Dir is direction vector (take into account any pair of coordinates with non-zero cross product)
If not, then it is necessary to calculate a distance between two skew lines at first. If the distance is zero, then we could find intersection point.