private static double GetQuartile(List<int> list, Point3DList pts,double quartile)
{
double result;
// Get roughly the index
double index = quartile * (list.Count() + 1);
// Get the remainder of that index value if exists
double remainder = index % 1;
// Get the integer value of that index
index = Math.Floor(index) - 1;
if (remainder.Equals(0))
{
// we have an integer value, no interpolation needed
result = pts[list.ElementAt((int)index)].Position.Xz.Length;
}
else
{
// we need to interpolate
double v1 = pts[list.ElementAt((int)index)].Position.Xz.Length;
double v2 = pts[list.ElementAt((int)(index + 1))].Position.Xz.Length;
result = v1 + ((v2 - v1) * remainder);
}
return result;
}
//public ~ICP();
public void Run(Point3DList varP, Point3DList varQ)
{
VarrQ = varQ;
VarrP = varP;
IniTransmat();
Sample();
//
float err = Closest();
//cout << "initial error = " << err << endl;
//
for (int i = 0; i < iterate; i++)
{
UpdateTransform();
T = Matrix4.Mult(T, W);
UpData();
float newerr = Closest();
// cout << "iterate times = " << i << endl;
// cout << "error = " << newerr << endl;
float delta = Math.Abs(err - newerr) / cono;
// cout << "delta = " << delta << endl;
if (delta < threshold)
break;
err = newerr;
}
ApplyAll();
}
Point3DList CreateListFromScanData(ScanData source)
{
Point3DList ret = new Point3DList();
IqrFilter filter = new IqrFilter();
filter.Factor = 0.5f;
ScanData data = filter.Run(source);
for (int i = 0; i < data.Count; i++)
ret.AddRange(data[i]);
return ret;
}
/**
Calculates a Bezier interpolated path for the given points.
*/
public void Interpolate(Point3DList segmentPoints, double scale)
{
controlPoints.Clear();
if (segmentPoints.Count < 2)
{
return;
}
for (int i = 0; i < segmentPoints.Count; i++)
{
if (i == 0) // is first
{
Point3D p1 = segmentPoints[i];
Point3D p2 = segmentPoints[i + 1];
controlPoints.Add(p1);
controlPoints.Add(Point3DList.Interpolate(p1,p2,scale));
}
else if (i == segmentPoints.Count - 1) //last
{
Point3D p0 = segmentPoints[i - 1];
Point3D p1 = segmentPoints[i];
controlPoints.Add(Point3DList.Interpolate(p0,p1, scale));
controlPoints.Add(p1);
}
else
{
Point3D p0 = segmentPoints[i - 1];
Point3D p1 = segmentPoints[i];
Point3D p2 = segmentPoints[i + 1];
Vector3d tp = (p2.Position - p0.Position).Normalized();
Vector3d pos0 = p1.Position - scale * tp * (p1.Position - p0.Position).Length;
Vector3d pos1 = p1.Position + scale * tp * (p2.Position - p1.Position).Length;
Vector3d tn = (p2.Normal - p0.Normal).Normalized();
Vector3d norm0 = p1.Normal - scale * tn * (p1.Normal - p0.Normal).Length;
Vector3d norm1 = p1.Normal + scale * tn * (p2.Normal - p1.Normal).Length;
controlPoints.Add(new Point3D(pos0,norm0,p0.Color.GetStepColor(p1.Color,scale)));
controlPoints.Add(p1);
controlPoints.Add(new Point3D(pos1, norm1, p1.Color.GetStepColor(p2.Color,scale)));
}
}
curveCount = (controlPoints.Count - 1) / 3;
}
public override void DoDamageBoat(BaseBoat boat)
{
if (boat == null)
{
return;
}
m_HasPushed = false;
IPoint2D pnt = boat;
if (Combatant != null && boat.Contains(Combatant))
{
pnt = Combatant;
}
Direction dir = Utility.GetDirection(this, pnt);
Point3DList path = new Point3DList();
for (int i = 0; i < DamageRange; i++)
{
int x = 0, y = 0;
switch ((int)dir)
{
case (int)Direction.Running:
case (int)Direction.North: { y -= i; break; }
case 129:
case (int)Direction.Right: { y -= i; x += i; break; }
case 130:
case (int)Direction.East: { x += i; break; }
case 131:
case (int)Direction.Down: { x += i; y += i; break; }
case 132:
case (int)Direction.South: { y += i; break; }
case 133:
case (int)Direction.Left: { y += i; x -= i; break; }
case 134:
case (int)Direction.West: { x -= i; break; }
case (int)Direction.ValueMask:
case (int)Direction.Up: { x -= i; y -= i; break; }
}
path.Add(X + x, Y + y, Z);
}
new EffectsTimer(this, path, dir, DamageRange);
}
Point3DList CreateListFromScanData(ScanData source)
{
Point3DList ret = new Point3DList();
IqrFilter filter = new IqrFilter();
filter.Factor = 0.5f;
ScanData data = filter.Run(source);
for (int i = 0; i < data.Count; i++)
{
ret.AddRange(data[i]);
}
return(ret);
}
public ValidityRange GetRange(Point3DList source,double factor)
{
List<int> indexes = new List<int>(source.Count);
for (int i = 0; i < source.Count; i++)
indexes.Add(i);
indexes.Sort(delegate(int x, int y)
{
return GetVal(source[x]).CompareTo( GetVal(source[y]));
}
);
double first = GetQuartile(indexes, source, 0.25);
double third = GetQuartile(indexes, source, 0.75);
double iqr = third - first;
ValidityRange ret = new ValidityRange();
ret.Min = first - factor * iqr;
ret.Max = third + factor * iqr;
return ret ;
}
public override ScanData DoTask(ScanData source)
{
ScanData ret = source;
Dictionary <int, ScanData> laserScanData = new Dictionary <int, ScanData>();
UpdatePercent(0, ret);
for (int i = 0; i < source.Count; i++)
{
ScanLine currentLine = source[i];
ScanData data = laserScanData.ContainsKey(currentLine.LaserID) ? laserScanData[currentLine.LaserID] : new ScanData();
data.Add(currentLine);
laserScanData[currentLine.LaserID] = data;
}
if (laserScanData.Keys.Count >= 1)
{
List <ScanData> datas = new List <ScanData>(laserScanData.Values);
int maxSamplePoints = 10000;
for (int i = 0; i < datas.Count; i++)
{
maxSamplePoints = Math.Min(maxSamplePoints, datas[i].PointCount());
}
//ICP.ICP icp = new ICP.ICP(maxSamplePoints/2, 0.001f, 10);
ICP.IterativeClosestPointTransform icp = new ICP.IterativeClosestPointTransform();
icp.NumberOfIterations = 1000;
icp.NumberOfStartTrialPoints = maxSamplePoints / 2;
ICP.IterativeClosestPointTransform.SimulatedAnnealing = false;
ICP.IterativeClosestPointTransform.DistanceOptimization = true;
Point3DList refpoints = CreateListFromScanData(datas[0]);
for (int i = 1; i < datas.Count; i++)
{
Point3DList points = CreateListFromScanData(datas[i]);
Matrix4d mat = icp.PerformICP(refpoints, points);
datas[i].Transform(mat);
//icp.Run(points, refpoints);
UpdatePercent((int)(100f * i / datas.Count), ret);
}
}
UpdatePercent(100, ret);
return(ret);
}
/// <summary>
/// Adjust normals for a "trianglestrip" Point list
/// </summary>
/// <param name="points"></param>
public static void AdjustNormalFromTriangleStrip(Point3DList points)
{
int count = points.Count;
if (count < 3)
{
return;
}
Vector3d normal = new Vector3d();
for (int i = 0; i < count - 2; i++)
{
Vector3d v0 = points[i].Position;
Vector3d v1 = points[i + 1].Position;
Vector3d v2 = points[i + 2].Position;
normal = -Triangle3D.CalculateNormal(v0, v1, v2);
points[i].Normal = normal;
}
points[count - 1].Normal = normal;
points[count - 2].Normal = normal;
}
public ValidityRange GetRange(Point3DList source, double factor)
{
List <int> indexes = new List <int>(source.Count);
for (int i = 0; i < source.Count; i++)
{
indexes.Add(i);
}
indexes.Sort(delegate(int x, int y)
{
return(GetVal(source[x]).CompareTo(GetVal(source[y])));
}
);
double first = GetQuartile(indexes, source, 0.25);
double third = GetQuartile(indexes, source, 0.75);
double iqr = third - first;
ValidityRange ret = new ValidityRange();
ret.Min = first - factor * iqr;
ret.Max = third + factor * iqr;
return(ret);
}
protected override StripResult CreateStrip(ScanLine previous, ScanLine current)
{
ScanLine ret1 = new ScanLine(previous.LaserID);
ScanLine ret2 = new ScanLine(current.LaserID);
Point3DList all = new Point3DList();
all.AddRange(previous);
all.AddRange(current.Where(p2 => previous.All(p1 => p1.Position.Y != p2.Position.Y)));
all.Sort();
all.Reverse();
// all.AddRange(list1.Union(list2, ));
for (int i = 0; i < all.Count; i++)
{
double y = all[i].Position.Y;
Point3D p1 = previous.GetNearestY(y);
Point3D p2 = current.GetNearestY(y);
ret1.Add(p1);
ret2.Add(p2);
}
return new StripResult(ret1, ret2);
}
protected override StripResult CreateStrip(ScanLine previous, ScanLine current)
{
ScanLine ret1 = new ScanLine(previous.LaserID);
ScanLine ret2 = new ScanLine(current.LaserID);
Point3DList all = new Point3DList();
all.AddRange(previous);
all.AddRange(current.Where(p2 => previous.All(p1 => p1.Position.Y != p2.Position.Y)));
all.Sort();
all.Reverse();
// all.AddRange(list1.Union(list2, ));
for (int i = 0; i < all.Count; i++)
{
double y = all[i].Position.Y;
Point3D p1 = previous.GetNearestY(y);
Point3D p2 = current.GetNearestY(y);
ret1.Add(p1);
ret2.Add(p2);
}
return(new StripResult(ret1, ret2));
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="laserID"></param>
/// <param name="points"></param>
public ScanLine(int laserID, Point3DList points)
: this(laserID, (IEnumerable <Point3D>)points)
{
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="points"></param>
public ScanSlice(Point3DList points)
: base(-1,points)
{
}
/**
This gets the drawing points of a bezier curve, using recursive division,
which results in less points for the same accuracy as the above implementation.
*/
public Point3DList GetDrawingPoints2()
{
Point3DList drawingPoints = new Point3DList();
for (int curveIndex = 0; curveIndex < curveCount; curveIndex++)
{
Point3DList bezierCurveDrawingPoints = FindDrawingPoints(curveIndex);
if (curveIndex != 0)
{
//remove the fist point, as it coincides with the last point of the previous Bezier curve.
bezierCurveDrawingPoints.RemoveAt(0);
}
drawingPoints.AddRange(bezierCurveDrawingPoints);
}
return drawingPoints;
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="points"></param>
public ScanSlice(Point3DList points)
: base(-1, points)
{
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="laserID"></param>
/// <param name="points"></param>
public ScanLine(int laserID, Point3DList points)
: this(laserID, (IEnumerable<Point3D>)points)
{
}
/** 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;
}
protected Point3D Smooth(int index, ScanLine prev, ScanLine current, ScanLine next)
{
//http://paulbourke.net/geometry/polygonmesh/
Point3DList nearPoints = new Point3DList();
if (index > 0)
{
Point3D prevP = current[index - 1];
nearPoints.Add(prevP);
nearPoints.Add(prev.GetInterpolateByY(prevP.Position.Y));
nearPoints.Add(next.GetInterpolateByY(prevP.Position.Y));
}
Point3D pt = current[index];
nearPoints.Add(pt);
nearPoints.Add(prev.GetInterpolateByY(pt.Position.Y));
nearPoints.Add(next.GetInterpolateByY(pt.Position.Y));
if (index <current.Count-1)
{
Point3D nextP = current[index + 1];
nearPoints.Add(nextP);
nearPoints.Add(prev.GetInterpolateByY(nextP.Position.Y));
nearPoints.Add(next.GetInterpolateByY(nextP.Position.Y));
}
Point3D ret = Smooth(current[index], nearPoints);
return ret;
}
private int curveCount; //how many bezier curves in this path?
/**
Constructs a new empty Bezier curve. Use one of these methods
to add points: SetControlPoints, Interpolate, SamplePoints.
*/
public BezierBuilder(int segmentPerCurve = 10, double minSquareDistance = 0.01f)
{
controlPoints = new Point3DList();
SEGMENTS_PER_CURVE = segmentPerCurve;
MINIMUM_SQR_DISTANCE = minSquareDistance;
}
private static void Write(StreamWriter w, Point3DList points)
{
for (int i = 0; i < points.Count; i++)
Write(w, points[i]);
}
/**
@returns the number of points added.
*/
int FindDrawingPoints(int curveIndex, double t0, double t1,
Point3DList pointList, int insertionIndex)
{
Point3D left = CalculateBezierPoint(curveIndex, t0);
Point3D right = CalculateBezierPoint(curveIndex, t1);
if ((left.Position - right.Position).LengthSquared < MINIMUM_SQR_DISTANCE)
{
return 0;
}
double tMid = (t0 + t1) / 2;
Point3D mid = CalculateBezierPoint(curveIndex, tMid);
Vector3d leftDirection = (left.Position - mid.Position).Normalized();
Vector3d rightDirection = (right.Position - mid.Position).Normalized();
if (Vector3d.Dot(leftDirection, rightDirection) > DIVISION_THRESHOLD || Math.Abs(tMid - 0.5f) < 0.0001f)
{
int pointsAddedCount = 0;
pointsAddedCount += FindDrawingPoints(curveIndex, t0, tMid, pointList, insertionIndex);
pointList.Insert(insertionIndex + pointsAddedCount, mid);
pointsAddedCount++;
pointsAddedCount += FindDrawingPoints(curveIndex, tMid, t1, pointList, insertionIndex + pointsAddedCount);
return pointsAddedCount;
}
return 0;
}
/**
Gets the drawing points. This implementation simply calculates a certain number
of points per curve.
*/
public Point3DList GetDrawingPoints0()
{
Point3DList drawingPoints = new Point3DList();
for (int curveIndex = 0; curveIndex < curveCount; curveIndex++)
{
if (curveIndex == 0) //Only do this for the first end point.
//When i != 0, this coincides with the
//end point of the previous segment,
{
drawingPoints.Add(CalculateBezierPoint(curveIndex, 0));
}
for (int j = 1; j <= SEGMENTS_PER_CURVE; j++)
{
double t = j / (double)SEGMENTS_PER_CURVE;
drawingPoints.Add(CalculateBezierPoint(curveIndex, t));
}
}
return drawingPoints;
}
/// <summary>
/// Adjust normals for a "trianglestrip" Point list
/// </summary>
/// <param name="points"></param>
public static void AdjustNormalFromTriangleStrip(Point3DList points)
{
int count = points.Count;
if (count < 3)
return;
Vector3d normal = new Vector3d();
for (int i = 0; i < count - 2; i++)
{
Vector3d v0 = points[i].Position;
Vector3d v1 = points[i + 1].Position;
Vector3d v2 = points[i + 2].Position;
normal = -Triangle3D.CalculateNormal(v0, v1, v2);
points[i].Normal = normal;
}
points[count - 1].Normal = normal;
points[count - 2].Normal = normal;
}
/// <summary>
/// Create Top and Bottom of mesh
/// </summary>
/// <param name="pts"></param>
/// <param name="invert"></param>
/// <returns></returns>
protected ScanSlice CreateForTopBottom(Point3DList pts, bool invert = false)
{
int count = pts.Count;
if (count < 2)
return new ScanSlice(pts);
ScanSlice outerList = new ScanSlice(count);
double x = 0;
double y = 0;
double z = 0;
for (int i = 0; i < count + 1; i++)
{
Point3D p = pts[i % count];
if (i == 0 || (pts[i - 1].Position - p.Position).LengthFast != 0)
{
outerList.Add(p);
if (i < count)
{
x += p.Position.X;
y += p.Position.Y;
z += p.Position.Z;
}
}
}
Vector3d center = new Vector3d((double)(x / count), (double)(y / count), (double)(z / count));
count = outerList.Count;
if (count < 2)
return outerList;
ScanSlice ret = new ScanSlice(count * 2);
int idx = 0;
for (idx = 0; idx < count; idx++)
{
Point3D pt = outerList[idx];
if (invert)
ret.Add(new Point3D(center, pt.Normal, pt.Color));
ret.Add(pt);
if (!invert)
ret.Add(new Point3D(center, pt.Normal, pt.Color));
}
return ret;
}
/**
Gets the drawing points. This implementation simply calculates a certain number
of points per curve.
This is a lsightly different inplementation from the one above.
*/
public Point3DList GetDrawingPoints1()
{
Point3DList drawingPoints = new Point3DList();
for (int i = 0; i < controlPoints.Count - 3; i += 3)
{
Point3D p0 = controlPoints[i];
Point3D p1 = controlPoints[i + 1];
Point3D p2 = controlPoints[i + 2];
Point3D p3 = controlPoints[i + 3];
if (i == 0) //only do this for the first end point. When i != 0, this coincides with the end point of the previous segment,
{
drawingPoints.Add(CalculateBezierPoint(0, p0, p1, p2, p3));
}
for (int j = 1; j <= SEGMENTS_PER_CURVE; j++)
{
double t = j / (double)SEGMENTS_PER_CURVE;
drawingPoints.Add(CalculateBezierPoint(t, p0, p1, p2, p3));
}
}
return drawingPoints;
}
Point3DList FindDrawingPoints(int curveIndex)
{
Point3DList pointList = new Point3DList();
Point3D left = CalculateBezierPoint(curveIndex, 0);
Point3D right = CalculateBezierPoint(curveIndex, 1);
pointList.Add(left);
pointList.Add(right);
FindDrawingPoints(curveIndex, 0, 1, pointList, 1);
return pointList;
}
public override ScanData DoTask(ScanData source)
{
if (!HardwareAvailable)
{
throw new Exception(string.Format("HardWare missing : TURNTABLE:{0} LASER:{1} CAMERA:{2}", HardwarePresentTrace(TurnTable), HardwarePresentTrace(Laser), HardwarePresentTrace(Camera)));
}
RotationStep = (double)Math.Round(TurnTable.MinimumRotation() + (15f - TurnTable.MinimumRotation()) * ((100 - Precision) / 100f), 2);
Settings settings = Settings.Get <Settings>();
CameraLoc.X = settings.Read(Settings.CAMERA, Settings.X, 0f);
CameraLoc.Y = settings.Read(Settings.CAMERA, Settings.Y, 270f);
CameraLoc.Z = settings.Read(Settings.CAMERA, Settings.Z, 70f);
double thres = settings.Read(Settings.LASER_COMMON, Settings.MAGNITUDE_THRESHOLD, 10);
int min = settings.Read(Settings.LASER_COMMON, Settings.MIN_WIDTH, 1);
int max = settings.Read(Settings.LASER_COMMON, Settings.MAX_WIDTH, 60);
ICameraProxy camera = Settings.Get <ICameraProxy>();
ImageProcessor = new ImageProcessor(thres, min, max);
SizeF tableSize = new SizeF(
(float)settings.Read(Settings.TABLE, Settings.DIAMETER, 20f),
(float)settings.Read(Settings.TABLE, Settings.HEIGHT, 15f)
);
Lasers = new List <LaserInfo>(LaserId.Length);
for (int i = 0; i < LaserId.Length; i++)
{
Lasers.Add(new LaserInfo(LaserId[i], CameraLoc, tableSize));
}
ScanData ret = new ScanData();
UpdatePercent(0, ret);
int fadeTime = settings.Read(Settings.LASER_COMMON, Settings.FADE_DELAY, 100);
TurnTable.InitialiseRotation();
int laserCount = Lasers.Count;
// Scan all laser location,
for (double currentAngle = 0; currentAngle < 360f; currentAngle += RotationStep)
{
if (this.CancelPending)
{
return(ret);
}
Laser.TurnAll(false); // All laser off
Thread.Sleep(fadeTime); // wait fade laser
Bitmap imgoff = GetCapture();
for (int laserIndex = 0; laserIndex < laserCount; laserIndex++)
{
Laser.Turn(Lasers[laserIndex].Id, true);
Thread.Sleep(fadeTime); // wait fade laser
Bitmap imgon = GetCapture();
Laser.Turn(Lasers[laserIndex].Id, false);
List <PointF> laserloc = ImageProcessor.Process(imgoff, imgon, null);
Point3DList samplePoints = Lasers[laserIndex].MapPoints(laserloc, UseTexture ? imgoff : null, UseCorrectionMatrix);
PositionPostProcess(ref samplePoints, -Utils.DEGREES_TO_RADIANS(currentAngle));
ScanLine line = new ScanLine(laserIndex, samplePoints);
line.DisplayAsLine = true;
ret.Add(line);
}
int percent = (int)((currentAngle / 360f) * 100f);
UpdatePercent(percent, ret);
TurnTable.Rotate(currentAngle, false);
}
LineSort lineSort = new LineSort();
ret = lineSort.Run(ret, CallerControl, this.Worker, this.WorkerArg);
if (!string.IsNullOrEmpty(FileName))
{
string path = Path.Combine(Program.UserDataPath, FileName);
ScanDataIO.Write(path, ret);
}
return(ret);
}
/** 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);
}
/**
Sets the control points of this Bezier path.
Points 0-3 forms the first Bezier curve, points
3-6 forms the second curve, etc.
*/
public void SetControlPoints(Point3DList newControlPoints)
{
controlPoints.Clear();
controlPoints.AddRange(newControlPoints);
curveCount = (controlPoints.Count - 1) / 3;
}
public void PositionPostProcess(ref Point3DList list, double rotation)
{
// Build the 2D rotation matrix to rotate in the XZ plane
double c = (double)Math.Cos(rotation);
double s = (double)Math.Sin(rotation);
double scale = 1f;
for (int iPt = 0; iPt < list.Count; iPt++)
{
// Location
Point3D p = list[iPt];
double x = scale * (p.Position.X * c + p.Position.Z * -s);
double y = scale * (p.Position.Y);
double z = scale * (p.Position.X * s + p.Position.Z * c);
// Normal
double nx = p.Normal.X * c + p.Normal.Z * -s;
double ny = p.Normal.Y;
double nz = p.Normal.X * s + p.Normal.Z * c;
p.Position.X = x;
p.Position.Y = y;
p.Position.Z = z;
p.Normal.X = nx;
p.Normal.Y = ny;
p.Normal.Z = nz;
list[iPt] = p;
}
}
public void OnTick(Point3DList path, Direction dir, int i)
{
if (path.Count > i)
{
Point3D point = path[i];
int o = i - 1;
Server.Effects.PlaySound(point, this.Map, 278);
Server.Effects.PlaySound(point, this.Map, 279);
for (int rn = 0; rn < (o * 2) + 1; rn++)
{
int y = 0, x = 0, y2 = 0, x2 = 0;
bool diag = false;
switch ((int)dir)
{
case (int)Direction.Running:
case (int)Direction.North: { x = x - o + rn; break; }
case 129:
case (int)Direction.Right: { x = x - o + rn; y = y - o + rn; break; }
case 130:
case (int)Direction.East: { y = y - o + rn; break; }
case 131:
case (int)Direction.Down: { y = y - o + rn; x = x + o - rn; break; }
case 132:
case (int)Direction.South: { x = x + o - rn; break; }
case 133:
case (int)Direction.Left: { x = x + o - rn; y = y + o - rn; break; }
case 134:
case (int)Direction.West: { y = y + o - rn; break; }
case (int)Direction.ValueMask:
case (int)Direction.Up: { y = y + o - rn; x = x - o + rn; break; }
}
switch ((int)dir)
{
case 129:
case (int)Direction.Right: { y2++; diag = true; break; }
case 131:
case (int)Direction.Down: { x2--; diag = true; break; }
case 133:
case (int)Direction.Left: { y2--; diag = true; break; }
case (int)Direction.ValueMask:
case (int)Direction.Up: { x2++; diag = true; break; }
default: { break; }
}
Point3D ep = new Point3D(point.X + x, point.Y + y, point.Z);
Point3D ep2 = new Point3D(ep.X + x2, ep.Y + y2, ep.Z);
if (diag && i >= ((2 * path.Count) / 3))
{
return;
}
Point3D p;
if (diag && rn < (o * 2))
{
p = ep2;
}
else
{
p = ep;
}
if (Spells.SpellHelper.CheckMulti(p, this.Map))
{
BaseGalleon galleon = BaseGalleon.FindGalleonAt(p, this.Map);
if (galleon != null && !m_HasPushed)
{
int damage = Utility.RandomMinMax(MinBoatDamage, MaxBoatDamage);
galleon.OnTakenDamage(this, damage);
galleon.StartMove(dir, 1, 0x2, galleon.SlowDriftInterval, true, false);
m_HasPushed = true;
}
continue;
}
LandTile t = this.Map.Tiles.GetLandTile(x, y);
if (IsSeaTile(t))
{
Mobile spawn = new EffectSpawn();
spawn.MoveToWorld(p, this.Map);
}
}
}
}
/**
Sample the given points as a Bezier path.
*/
public void SamplePoints(Point3DList sourcePoints, double minSqrDistance, double maxSqrDistance, double scale)
{
if (sourcePoints.Count < 2)
{
return;
}
Stack<Point3D> samplePoints = new Stack<Point3D>();
samplePoints.Push(sourcePoints[0]);
Point3D potentialSamplePoint = sourcePoints[1];
int i = 2;
for (i = 2; i < sourcePoints.Count; i++)
{
if (
((potentialSamplePoint.Position - sourcePoints[i].Position).LengthSquared > minSqrDistance) &&
((samplePoints.Peek().Position - sourcePoints[i].Position).LengthSquared > maxSqrDistance))
{
samplePoints.Push(potentialSamplePoint);
}
potentialSamplePoint = sourcePoints[i];
}
//now handle last bit of curve
Point3D p1 = samplePoints.Pop(); //last sample point
Point3D p0 = samplePoints.Peek(); //second last sample point
Vector3d posT = (p0.Position - potentialSamplePoint.Position).Normalized();
double pos_d2 = (potentialSamplePoint.Position - p1.Position).Length;
double pos_d1 = (p1.Position - p0.Position).Length;
double pos_scale =((pos_d1 - pos_d2) / 2f);
Vector3d pos_ = p1.Position + posT * pos_scale;
Vector3d normT = (p0.Normal - potentialSamplePoint.Normal).Normalized();
double norm_d2 = (potentialSamplePoint.Normal - p1.Normal).Length;
double norm_d1 = (p1.Normal - p0.Normal).Length;
Vector3d norm_ = p1.Normal + normT * ((norm_d1 - norm_d2) / 2);
samplePoints.Push(new Point3D(pos_,norm_,p1.Color.GetStepColor(p0.Color,pos_scale)));
samplePoints.Push(potentialSamplePoint);
Point3DList l = new Point3DList(samplePoints.Count);
l.AddRange(samplePoints);
Interpolate(l, scale);
}
