private static void Write(StreamWriter w, Point3D point)
{
string line = string.Format("{0} {1} {2} {3} {4}", point.Position.Dump(),
point.Normal.Dump(),
point.Color.R, point.Color.G, point.Color.B);
w.WriteLine(line);
}
/// <summary>
/// Distance to another Point3D
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public double DistanceSquared(Point3D other)
{
double dx = Position.X - other.Position.X;
double dy = Position.Y - other.Position.Y;
double dz = Position.Z - other.Position.Z;
return dx * dx + dy * dy + dz * dz;
}
private static void Write(StreamWriter w, Point3D point)
{
// string line = string.Format("{0} {1} {2} {3} {4} {5} {6} {7} {8}", point.Position.X, point.Position.Y, point.Position.Z,
// point.Normal.X, point.Normal.Y, point.Normal.Z,
// point.Color.R, point.Color.G, point.Color.B);
string line = point.Position.Dump();
w.WriteLine(line);
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="color"></param>
public Triangle3D(Point3D p1, Point3D p2, Point3D p3, Vector3d normal, Color color)
{
Point1 = p1;
Point2 = p2;
Point3 = p3;
Normal = normal;
this.Color = color;
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="color"></param>
public Triangle3D(Point3D p1, Point3D p2, Point3D p3, Color color)
: this(p1, p2, p3, CalculateNormal(p1, p2, p3), color)
{
}
/// <summary>
/// Average this point with another
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public Point3D Average(Point3D other)
{
Vector3d vp1 = this.Position;
Vector3d vp2 = other.Position;
Vector3d np1 = this.Normal;
Vector3d np2 = other.Normal;
Vector4 cp1 = this.Color.ToVector();
Vector4 cp2 = other.Color.ToVector();
vp1 = (vp2 + vp1) / 2f;
np1 = (np2 + np1) / 2f;
cp1 = (cp2 + cp1) / 2f;
return new Point3D(vp1, np1, ColorExtension.ColorFromVector(cp1));
}
/// <summary>
/// Remove the item with the smallest key from the queue.
/// </summary>
public void RemoveMin()
{
// Check for errors.
if (Size == 0)
throw new Exception();
// Remove the item by
Size--;
tData[0] = tData[Size];
tKeys[0] = tKeys[Size];
tData[Size] = new Point3D();
SiftDownMin(0);
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="normal"></param>
public Triangle3D(Point3D p1, Point3D p2, Point3D p3,Vector3d normal)
: this(p1, p2, p3, normal, CalculateColor(p1, p2, p3))
{
}
/// <summary>
/// Insert a new point into this leaf node.
/// </summary>
/// <param name="tPoint">The position which represents the data.</param>
/// <param name="kValue">The value of the data.</param>
public void AddPoint(Point3D kValue)
{
Vector3d tPoint = kValue.Position;
// Find the correct leaf node.
KDNode pCursor = this;
while (!pCursor.IsLeaf)
{
// Extend the size of the leaf.
pCursor.ExtendBounds(tPoint);
pCursor.Size++;
// If it is larger select the right, or lower, select the left.
if (tPoint[pCursor.iSplitDimension] > pCursor.fSplitValue)
{
pCursor = pCursor.pRight;
}
else
{
pCursor = pCursor.pLeft;
}
}
// Insert it into the leaf.
pCursor.AddLeafPoint(tPoint, kValue);
}
/**
Caluclates a point on the Bezier curve represented with the four controlpoints given.
*/
private Point3D CalculateBezierPoint(double t, Point3D p0, Point3D p1, Point3D p2, Point3D p3)
{
double u = 1 - t;
double tt = t * t;
double uu = u * u;
double uuu = uu * u;
double ttt = tt * t;
Vector3d p = uuu * p0.Position; //first term
p += 3 * uu * t * p1.Position; //second term
p += 3 * u * tt * p2.Position; //third term
p += ttt * p3.Position; //fourth term
Vector3d n = uuu * p0.Normal; //first term
n += 3 * uu * t * p1.Normal; //second term
n += 3 * u * tt * p2.Normal; //third term
n += ttt * p3.Normal; //fourth term
Vector4 c = ((float)uuu) * p0.Color.ToVector(); //first term
c += ((float)(3 * uu * t)) * p1.Color.ToVector(); //second term
c += ((float)(3 * u * tt)) * p2.Color.ToVector(); //third term
c += ((float)(ttt)) * p3.Color.ToVector(); //fourth term
return new Point3D(p, n, ColorExtension.ColorFromVector(c));
}
/// <summary>
/// Remove the item with the largest key in the queue.
/// </summary>
public void RemoveMax()
{
// If we have no items in the queue.
if (Size == 0)
{
throw new Exception();
}
// If we have one item, remove the min.
else if (Size == 1)
{
RemoveMin();
return;
}
// Remove the max.
Size--;
tData[1] = tData[Size];
tKeys[1] = tKeys[Size];
tData[Size] = new Point3D();
SiftDownMax(1);
}
/// <summary>
/// Replace the item with the smallest key in the queue.
/// </summary>
/// <param name="key">The new minimum key.</param>
/// <param name="value">The new minumum data value.</param>
public void ReplaceMin(double key, Point3D value)
{
// Check for errors.
if (Size == 0)
throw new Exception();
// Add the data.
tData[0] = value;
tKeys[0] = key;
// If we have more than one item.
if (Size > 1)
{
// Swap with pair if necessary.
if (tKeys[1] < key)
Swap(0, 1);
SiftDownMin(0);
}
}
/// <summary>
/// Check for the next iterator item.
/// </summary>
/// <returns>True if we have one, false if not.</returns>
public bool MoveNext()
{
// Bail if we are finished.
if (iPointsRemaining == 0)
{
_Current = new Point3D();
return false;
}
// While we still have paths to evaluate.
while (pPending.Size > 0 && (pEvaluated.Size == 0 || (pPending.MinKey < pEvaluated.MinKey)))
{
// If there are pending paths possibly closer than the nearest evaluated point, check it out
KDNode pCursor = pPending.Min;
pPending.RemoveMin();
// Descend the tree, recording paths not taken
while (!pCursor.IsLeaf)
{
KDNode pNotTaken;
// If the seach point is larger, select the right path.
if (tSearchPoint[pCursor.iSplitDimension] > pCursor.fSplitValue)
{
pNotTaken = pCursor.pLeft;
pCursor = pCursor.pRight;
}
else
{
pNotTaken = pCursor.pRight;
pCursor = pCursor.pLeft;
}
// Calculate the shortest distance between the search point and the min and max bounds of the kd-node.
double fDistance = kDistanceFunction.DistanceToRectangle(tSearchPoint, pNotTaken.tMinBound, pNotTaken.tMaxBound);
// If it is greater than the threshold, skip.
if (fThreshold >= 0 && fDistance > fThreshold)
{
//pPending.Insert(fDistance, pNotTaken);
continue;
}
// Only add the path we need more points or the node is closer than furthest point on list so far.
if (pEvaluated.Size < iPointsRemaining || fDistance <= pEvaluated.MaxKey)
{
pPending.Insert(fDistance, pNotTaken);
}
}
// If all the points in this KD node are in one place.
if (pCursor.bSinglePoint)
{
// Work out the distance between this point and the search point.
double fDistance = kDistanceFunction.Distance(pCursor.tPoints[0], tSearchPoint);
// Skip if the point exceeds the threshold.
// Technically this should never happen, but be prescise.
if (fThreshold >= 0 && fDistance >= fThreshold)
continue;
// Add the point if either need more points or it's closer than furthest on list so far.
if (pEvaluated.Size < iPointsRemaining || fDistance <= pEvaluated.MaxKey)
{
for (int i = 0; i < pCursor.Size; ++i)
{
// If we don't need any more, replace max
if (pEvaluated.Size == iPointsRemaining)
pEvaluated.ReplaceMax(fDistance, pCursor.tData[i]);
// Otherwise insert.
else
pEvaluated.Insert(fDistance, pCursor.tData[i]);
}
}
}
// If the points in the KD node are spread out.
else
{
// Treat the distance of each point seperately.
for (int i = 0; i < pCursor.Size; ++i)
{
// Compute the distance between the points.
double fDistance = kDistanceFunction.Distance(pCursor.tPoints[i], tSearchPoint);
// Skip if it exceeds the threshold.
if (fThreshold >= 0 && fDistance >= fThreshold)
continue;
// Insert the point if we have more to take.
if (pEvaluated.Size < iPointsRemaining)
pEvaluated.Insert(fDistance, pCursor.tData[i]);
// Otherwise replace the max.
else if (fDistance < pEvaluated.MaxKey)
pEvaluated.ReplaceMax(fDistance, pCursor.tData[i]);
}
}
}
// Select the point with the smallest distance.
if (pEvaluated.Size == 0)
return false;
iPointsRemaining--;
_CurrentDistance = pEvaluated.MinKey;
_Current = pEvaluated.Min;
pEvaluated.RemoveMin();
return true;
}
public double GetVal(Point3D pt)
{
return pt.Position.Xz.Length;
}
/** Lookup the 3D points for each pixel location */
public Point3DList MapPoints(List<PointF> laserLocations, Bitmap image, Color defColor)
{
double MAX_DIST_Y = TableSize.Height * 2;
double MAX_DIST_XZ_SQ = (TableSize.Width / 2) * (TableSize.Width / 2);
Point3DList points = new Point3DList(laserLocations.Count);
int numIntersectionFails = 0;
int numDistanceFails = 0;
Ray ray;
bool haveImage = image != null;
// Initialize our output variable
for (int iLoc = 0; iLoc < laserLocations.Count; iLoc++)
{
// Compute the back projection ray
ray = CalculateCameraRay(laserLocations[iLoc]);
// Intersect the laser plane and populate the XYZ
Point3D point = new Point3D();
if (IntersectLaserPlane(ray, ref point, laserLocations[iLoc]))
{
// The point must be above the turn table and less than the max distance from the center of the turn table
double distXZSq = point.Position.X * point.Position.X + point.Position.Z * point.Position.Z;
if (point.Position.Y >= 0.0 && distXZSq < MAX_DIST_XZ_SQ && point.Position.Y < MAX_DIST_Y)
{
// Set the color
if (haveImage)
{
point.Color = image.GetPixel(Utils.ROUND(laserLocations[iLoc].X), Utils.ROUND(laserLocations[iLoc].Y));
}
else
point.Color = defColor;
// Make sure we have the correct laser location
laserLocations[points.Count] = laserLocations[iLoc];
points.Add(point);
}
else
{
numDistanceFails++;
}
}
else
{
numIntersectionFails++;
}
}
if (numIntersectionFails > 0)
{
Debug.WriteLine("!! " + numIntersectionFails + " laser plane intersection failures.");
}
if (numDistanceFails > 0)
{
Debug.WriteLine("!! " + numDistanceFails + " object bounds failures. ");
}
return points;
}
/** 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;
}
protected Point3D Smooth(Point3D p, IList<Point3D> nearPts)
{
if(nearPts.Count<1)
return p;
Vector3d pos = new Vector3d(0, 0, 0);
Vector3d norm = new Vector3d(0, 0, 0);
int count =nearPts.Count;
for (int i = 0; i < count; i++)
{
pos = pos + (nearPts[i].Position - p.Position);
norm = norm + (nearPts[i].Normal - p.Normal);
}
pos = p.Position + pos / count;
norm = p.Normal + norm / count;
norm.Normalize();
return new Point3D(pos,norm,p.Color);
}
private static Point3D CalculateCenterOfGravity(this IList<Point3D> vertices)
{
Point3D centerOfGravity = new Point3D();
foreach (Point3D vr in vertices)
centerOfGravity.Position += vr.Position;
centerOfGravity.Position /= (double)vertices.Count;
return centerOfGravity;
}
/// <summary>
/// Calculate a normal for 3 points
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <returns></returns>
public static Vector3d CalculateNormal(Point3D a, Point3D b, Point3D c)
{
return CalculateNormal(a.Position, b.Position, c.Position);
}
public static void AddVector(this IList<Point3D> vertices, Point3D newOrigin)
{
//reset vertex so that it starts from 0,0,0
for (int i = 0; i < vertices.Count; i++)
{
Vector3d v = vertices[i].Position;
v.X += newOrigin.Position.X;
v.Y += newOrigin.Position.Y;
v.Z += newOrigin.Position.Z;
vertices[i].Position = v;
}
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
public Triangle3D(Point3D p1, Point3D p2, Point3D p3)
:this(p1,p2,p3,CalculateNormal(p1,p2,p3),CalculateColor(p1, p2, p3))
{
}
/// <summary>
/// Read ScanData File
/// </summary>
/// <param name="file"></param>
/// <returns></returns>
public static ScanData Read(string file)
{
ScanData ret = new ScanData();
using (StreamReader r = System.IO.File.OpenText(file))
{
string line = r.ReadLine();
string[] part = line.Split(":".ToArray());
int slicecount = int.Parse(part[1]);
for (int i = 0; i < slicecount; i++)
{
line = r.ReadLine();
part = line.Split(":".ToArray());
ScanLine slice = new ScanLine((int)double.Parse(part[1]));
line = r.ReadLine();
part = line.Split(":".ToArray());
if (part[0] == "DrawAs")
{
OpenTK.Graphics.OpenGL.PrimitiveType primitive = OpenTK.Graphics.OpenGL.PrimitiveType.Points;
Enum.TryParse<OpenTK.Graphics.OpenGL.PrimitiveType>(part[1], true, out primitive);
switch (primitive)
{
case OpenTK.Graphics.OpenGL.PrimitiveType.TriangleStrip:
{
slice = new ScanSlice(10000);
break;
}
case OpenTK.Graphics.OpenGL.PrimitiveType.LineStrip:
{
slice.DisplayAsLine = true;
break;
}
default:
{
slice.DisplayAsLine = false;
break;
}
}
line = r.ReadLine();
part = line.Split(":".ToArray());
}
int pointcount = int.Parse(part[1]);
for (int j = 0; j < pointcount; j++)
{
line = r.ReadLine();
part = line.Split("|".ToArray());
Vector3d pos = GetVector(part[0]);
Vector3d normal = pos.Normalized();
try
{
normal = GetVector(part[1]);
}
catch { }
Color color = System.Drawing.ColorTranslator.FromHtml(part[2]);
Point3D p = new Point3D(pos, normal, color);
slice.Add(p);
}
ret.Add(slice);
}
}
return ret;
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="color"></param>
public Triangle3D(Point3D p1, Point3D p2, Point3D p3, Color color)
: this(p1, p2, p3, CalculateNormal(p1, p2, p3), color)
{
}
/// <summary>
/// Insert the point into the leaf.
/// </summary>
/// <param name="tPoint">The point to insert the data at.</param>
/// <param name="kValue">The value at the point.</param>
private void AddLeafPoint(Vector3d position,Point3D kValue)
{
// Add the data point to this node.
tPoints[Size] = position;
tData[Size] = kValue;
ExtendBounds(position);
Size++;
// Split if the node is getting too large in terms of data.
if (Size == tPoints.Length - 1)
{
// If the node is getting too physically large.
if (CalculateSplit())
{
// If the node successfully had it's split value calculated, split node.
SplitLeafNode();
}
else
{
// If the node could not be split, enlarge node data capacity.
IncreaseLeafCapacity();
}
}
}
/// <summary>
/// Swap out the item with the largest key in the queue.
/// </summary>
/// <param name="key">The new key for the largest item.</param>
/// <param name="value">The new data for the largest item.</param>
public void ReplaceMax(double key, Point3D value)
{
if (Size == 0)
{
throw new Exception();
}
else if (Size == 1)
{
ReplaceMin(key, value);
return;
}
tData[1] = value;
tKeys[1] = key;
// Swap with pair if necessary
if (key < tKeys[0])
{
Swap(0, 1);
}
SiftDownMax(1);
}
/// <summary>
/// Calculate a color based on 3 Points (average)
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <returns></returns>
public static Color CalculateColor(Point3D p1, Point3D p2, Point3D p3)
{
return CalculateColor(p1.Color, p2.Color, p3.Color);
}
/// <summary>
/// Calculate a normal for 3 points
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <returns></returns>
public static Vector3d CalculateNormal(Point3D a, Point3D b, Point3D c)
{
return CalculateNormal(a.Position, b.Position, c.Position);
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="normal"></param>
public Triangle3D(Point3D p1, Point3D p2, Point3D p3,Vector3d normal)
: this(p1, p2, p3, normal, CalculateColor(p1, p2, p3))
{
}
/// <summary>
/// Calculate a color based on 3 Points (average)
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <returns></returns>
public static Color CalculateColor(Point3D p1, Point3D p2, Point3D p3)
{
return CalculateColor(p1.Color, p2.Color, p3.Color);
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
public Triangle3D(Point3D p1, Point3D p2, Point3D p3)
:this(p1,p2,p3,CalculateNormal(p1,p2,p3),CalculateColor(p1, p2, p3))
{
}
/// <summary>
/// Insert a new data item at a given key.
/// </summary>
/// <param name="key">The value which represents our data (i.e. a distance).</param>
/// <param name="value">The data we want to store.</param>
public void Insert(double key, Point3D value)
{
// If more room is needed, double the array size.
if (Size >= Capacity)
{
// Double the capacity.
Capacity *= 2;
// Expand the data array.
var newData = new Point3D[Capacity];
Array.Copy(tData, newData, tData.Length);
tData = newData;
// Expand the key array.
var newKeys = new double[Capacity];
Array.Copy(tKeys, newKeys, tKeys.Length);
tKeys = newKeys;
}
// Insert the new value at the end.
Size++;
tData[Size - 1] = value;
tKeys[Size - 1] = key;
// Ensure it is in the right place.
SiftInsertedValueUp();
}
