Math – 3D line-triangle intersection

The following is a very simple but effective method I found to find the intersection point of a line with a triangle. I used it to determine the height of a point on the terrain.

The first part finds the intersection point:

private float? GetIntersectionY(Vector3 a, Vector3 b, Vector3 c, Vector3 pp) {
  //First, define a ray that will definitely intersect if the triangle lies beneath the point.
  var p = new Vector3(pp.X, 100, pp.Z);
  var q = new Vector3(p.X, p.Y - 100, p.Z);

  //Then, find the intersection.
  var normal = Vector3.CrossProduct(b - a, c - a).Normalized();
  float dist1 = Vector3.DotProduct(p - a, normal);
  float dist2 = Vector3.DotProduct(q - a, normal);

  if (dist1 == dist2 || dist1 * dist2 >= 0) //Parallel; no intersection.
    return null;

  var intersection = p + (q - p) * (-dist1 / (dist2 - dist1));

  //Verify that this point lies inside the triangle.
  if (SameSide(p, a, b, c) && SameSide(p, b, a, c) && SameSide(p, c, a, b))
    return intersection.Y;
  else
    return null;
}

The second part, as the above function already indicates, determines that the point really lies within the triangle, and not just within the mathematical plane. The linked sited explains well how this works.

private static bool SameSide(Vector3 p1, Vector3 p2, Vector3 a, Vector3 b) {
  //It only worked right for me when all the points had the same height though.
  var _p1 = new Vector3(p1.X, 0, p1.Z);
  var _p2 = new Vector3(p2.X, 0, p2.Z);
  var _a = new Vector3(a.X, 0, a.Z);
  var _b = new Vector3(b.X, 0, b.Z);

  //The check.
  var cp1 = Vector3.CrossProduct(_b - _a, _p1 - _a);
  var cp2 = Vector3.CrossProduct(_b - _a, _p2 - _a);
  return Vector3.DotProduct(cp1, cp2) >= 0;
}

Math – Align a rotation matrix with a normal

Angle and axis

I found this method when I needed to rotate a vehicle to be positioned on the terrain. I had the positions of the wheels, and used those to calculate the vehicle's normal. Then I found the following method to calculate the angle and rotation axis:

var axis = new Vector3(0, 1, 0); //Up vector
float angle = Vector3.DotProduct(normal, axis).Acos(); //I do like extension methods.
var transformation = new Matrix();

if (angle.Absolute() > 0.001f) {
  axis = Vector3.CrossProduct(normal, axis).Normalized();
  transformation.Rotate(angle, axis); //See below.
}

Rotate the matrix

Then the glRotatef function is used to use these values, but that's from the old fixed pipeline OpenGL, and I was using the new OpenGL with shaders and all that. I then found this function that applies the rotation to a matrix:

float rad = angle.ToRadians();
float c = rad.Cos();
float s = rad.Sin();
float t = 1 - c;

float x = axis.X;
float y = axis.Y;
float z = axis.Z;

var rotation = new Matrix();
rotation.M[0, 0] = c + x * x * t;
rotation.M[0, 1] = y * x * t + z * s;
rotation.M[0, 2] = z * x * t - y * s;

rotation.M[1, 0] = x * y * t - z * s;
rotation.M[1, 1] = c + y * y * t;
rotation.M[1, 2] = z * y * t + x * s;

rotation.M[2, 0] = x * z * t + y * s;
rotation.M[2, 1] = y * z * t - x * s;
rotation.M[2, 2] = z * z * t + c;

SetMatrix((this * rotation).M); //This just overwrites the 16 values of this matrix.

See also

Using quaternions to create a rotation matrix