/// <summary>
/// Is the a Ray intersecting this plane
/// </summary>
/// <param name="inRay"></param>
/// <param name="intersection"></param>
/// <returns></returns>
public bool Intersect(Ray inRay, out Vector3d intersection)
{
intersection = new Vector3d();
// Reference: http://www.scratchapixel.com/lessons/3d-basic-lessons/lesson-7-intersecting-simple-shapes/ray-plane-and-ray-disk-intersection/
// d = ((p0 - l0) * n) / (l * n)
Ray ray = inRay;
// Negate Z since this is a RH coordinate system
ray.Direction.Z *= -1;
// If dn is close to 0 then they don't intersect. This should never happen
double denominator = ray.Direction.Dot(Normal);
if (Math.Abs(denominator) < 0.00001)
{
return false;
}
Vector3d v;
v.X = Point.X - ray.Origin.X;
v.Y = Point.Y - ray.Origin.Y;
v.Z = -1 * (Point.Z - ray.Origin.Z);
double numerator = v.Dot(Normal);
// Compute the distance along the ray to the plane
double d = numerator / denominator;
if (d < 0)
{
return false;
}
// Extend the ray out this distance
intersection.X = inRay.Origin.X + (inRay.Direction.X * d);
intersection.Y = inRay.Origin.Y + (inRay.Direction.Y * d);
intersection.Z = inRay.Origin.Z + (inRay.Direction.Z * d);
return true;
}
/** Calculate where the ray will hit the laser plane and write it to @p point */
private bool IntersectLaserPlane(Ray ray, ref Point3D point, PointF pixel)
{
// Reference: http://www.scratchapixel.com/lessons/3d-basic-lessons/lesson-7-intersecting-simple-shapes/ray-plane-and-ray-disk-intersection/
// d = ((p0 - l0) * n) / (l * n)
// If dn is close to 0 then they don't intersect. This should never happen
double denominator = ray.Direction.Dot(m_LaserPlane.Normal);
if (Math.Abs(denominator) < 0.000001)
{
Debug.WriteLine("!!! Ray never hits laser plane, pixel=" + pixel.X + ", " + pixel.Y + ", laserX=" + m_LaserPos.X + ", denom=" + denominator);
return false;
}
Vector3d v;
v.X = m_LaserPlane.Point.X - ray.Origin.X;
v.Y = m_LaserPlane.Point.Y - ray.Origin.Y;
v.Z = m_LaserPlane.Point.Z - ray.Origin.Z;
double numerator = v.Dot(m_LaserPlane.Normal);
// Compute the distance along the ray to the plane
double d = numerator / denominator;
if (d < 0)
{
// The ray is going away from the plane. This should never happen.
Debug.WriteLine("!!! Back projection ray is going the wrong direction! Ray Origin = (" +
ray.Origin.X + "," + ray.Origin.Y + "," + ray.Origin.Z + ") Direction = (" +
ray.Direction.X + "," + ray.Direction.Y + "," + ray.Direction.Z + ")");
return false;
}
// Extend the ray out this distance
point.Position.X = ray.Origin.X + (ray.Direction.X * d);
point.Position.Y = ray.Origin.Y + (ray.Direction.Y * d);
point.Position.Z = ray.Origin.Z + (ray.Direction.Z * d);
point.Normal.X = m_LaserPos.X - point.Position.X;
point.Normal.Y = m_LaserPos.Y - point.Position.Y;
point.Normal.Z = m_LaserPos.Z - point.Position.Z;
point.Normal.Normalize();
return true;
}
/**
* Calculates a camera back projection Ray for the given image point. The ray
* will begin at globalPt and extend out into the scene with a vector that would also
* take it through the camera focal point.
* @param globalPt - Point on the camera sensor in global coordinates
* @param ray - The output ray.
*/
private Ray CalculateCameraRay(PointF imagePixel)
{
// Performance Note: Most of this could be pre-computed
// and all the division could be removed.
// and distance to camera
// We subtract by one because the image is 0 indexed
double x = imagePixel.X / (double)(m_Image.Width - 1);
// Subtract the height so it goes from bottom to top
double y = (m_Image.Height - imagePixel.Y) / (double)(m_Image.Height - 1);
// The center of the sensor is at 0 in the X dimension
x = (x * m_Sensor.Width) - (m_Sensor.Width * 0.5f) + m_CameraPos.X;
y = (y * m_Sensor.Height) - (m_Sensor.Height * 0.5f) + m_CameraPos.Y;
double z = m_CameraPos.Z - m_focalLength;
Ray ray = new Ray(new Vector3d(x, y, z), new Vector3d(x - m_CameraPos.X, y - m_CameraPos.Y, z - m_CameraPos.Z));
ray.Direction.Normalize();
return ray;
}