public DrawingObject()
{
points = new List<PointDouble>();
lines = new List<Tuple<PointDouble, PointDouble>>();
_moments = new List<Tuple<PointDouble, double>>();
_forces = new List<Tuple<PointDouble, double, double>>();
_centerOfMass = new PointDouble ();
}
public void AddAnchor(PointDouble pt)
{
DrawingObject body;
if (!_pointToObject.TryGetValue(pt, out body))
return;
RigidBodyController controller;
if (_bodyToController.TryGetValue(body, out controller))
{
//controller.addAnchor(pt);
}
}
public void AddPoint(PointDouble point)
{
if (points.Count == 0)
firstPoint = point;
points.Add(point);
if (points.Count > 1)
lines.Add(new Tuple<PointDouble, PointDouble>(lastPoint, point));
lastPoint = point;
CalcCenterOfMass();
}
public void CorrectnessTest()
{
SpatialTree tree = new SpatialTree ();
List<PointDouble> points = new List<PointDouble> ();
Random rand = new Random ();
//initialize the list
for (int i = 0; i < 10000; ++i)
{
points.Add (new PointDouble (rand.Next (1000), rand.Next (1000)));
}
//insert them all into the list
foreach (PointDouble pt in points)
{
tree.addPoint (pt);
}
List<PointDouble> found1;
List<PointDouble> found2;
for (int i = 0; i < 10000; ++i)
{
var point = new PointDouble(rand.Next(1000), rand.Next(1000));
found1 = tree.GetPointsInRange( point, 10);
found2 = new List<PointDouble>();
foreach(PointDouble pt in points)
{
if ((pt.X-point.X)*(pt.X-point.X) +
(pt.Y-point.Y)*(pt.Y-point.Y) < 100.0)
{
found2.Add(pt);
}
}
foreach(PointDouble pt in found1)
{
Assert.AreEqual(true, found2.Contains(pt));
}
foreach(PointDouble pt in found2)
{
Assert.AreEqual(true, found1.Contains(pt));
}
//found1.Clear();
//found2.Clear ();
}
}
public void GetClosestTest()
{
SpatialTree tree = new SpatialTree ();
List<PointDouble> points = new List<PointDouble> ();
Random rand = new Random ();
//initialize the list
for (int i = 0; i < 10000; ++i)
{
points.Add (new PointDouble (rand.Next (1000), rand.Next (1000)));
}
//insert them all into the list
foreach (PointDouble pt in points)
{
tree.addPoint (pt);
}
for (int i = 0; i < 10000; ++i)
{
var point = new PointDouble(rand.Next(1000), rand.Next(1000));
var found = tree.GetClosestPoint(point);
PointDouble closestPoint = new PointDouble();
double distanceSq = double.MaxValue;
foreach(PointDouble pt in points)
{
if ((pt.X-point.X)*(pt.X-point.X) +
(pt.Y-point.Y)*(pt.Y-point.Y) < distanceSq)
{
closestPoint = pt;
distanceSq = (pt.X-point.X)*(pt.X-point.X) + (pt.Y-point.Y)*(pt.Y-point.Y);
}
}
var dist = (found.X - point.X)*(found.X - point.X) + (found.Y - point.Y)*(found.Y - point.Y);
Assert.AreEqual(distanceSq, dist);
}
}
public void AddJoint(PointDouble pt1, PointDouble pt2)
{
DrawingObject body1, body2;
if (!_pointToObject.TryGetValue(pt1, out body1) ||
!_pointToObject.TryGetValue(pt2, out body2))
return;
// _spatialTree.remove(pt2);
_pointToObject.Remove(pt2);
var newLines = new List<Tuple<PointDouble, PointDouble>>();
var oldLines = new List<Tuple<PointDouble, PointDouble>>();
foreach(var line in body2.lines)
{
if (line.Item1 == pt2)
{
newLines.Add(new Tuple<PointDouble, PointDouble>(pt1, line.Item2));
oldLines.Add(line);
}
else if (line.Item2 == pt2)
{
newLines.Add(new Tuple<PointDouble, PointDouble>(line.Item1, pt1));
oldLines.Add(line);
}
}
foreach (var line in oldLines)
{
body2.lines.Remove(line);
}
body2.lines.AddRange(newLines);
body2.points.Remove(pt2);
body2.points.Add(pt1);
}
public void StressTest()
{
SpatialTree tree = new SpatialTree ();
List<PointDouble> points = new List<PointDouble> ();
Random rand = new Random ();
//initialize the list
for (int i = 0; i < 10000; ++i)
{
points.Add (new PointDouble (rand.Next (1000), rand.Next (1000)));
}
DateTime start = DateTime.Now;
//insert them all into the list
foreach (PointDouble pt in points)
{
tree.addPoint (pt);
}
DateTime stop = DateTime.Now;
Console.WriteLine ("Took " + (stop - start) + " to add " + 10000 + " records");
start = DateTime.Now;
for (int i = 0; i < 10000; ++i)
{
tree.GetPointsInRange(new PointDouble(rand.Next(1000), rand.Next(1000)), 10);
}
stop = DateTime.Now;
var treeTime = stop-start;
Console.WriteLine("SpatialTree: Took " + treeTime + " to lookup " + 10000 + " records");
start = DateTime.Now;
List<PointDouble> found;
for (int i = 0; i < 10000; ++i)
{
found = new List<PointDouble>();
var lookup = new PointDouble(rand.Next(1000), rand.Next(1000));
foreach(PointDouble pt in points)
{
if ((pt.X-lookup.X)*(pt.X-lookup.X) +
(pt.Y-lookup.Y)*(pt.Y-lookup.Y) < 100)
{
found.Add(pt);
}
}
}
stop = DateTime.Now;
var linearTime = stop-start;
Console.WriteLine("Linear search: Took " + linearTime+ " to lookup " + 10000 + " records");
Assert.Greater(linearTime, treeTime);
}
public void addPoint(PointDouble point, Node node)
{
//if we have reached a leaf node
if (node == null)
node = new Node ();
if (node._left == null && node._right == null) {
node._bucket.Add (point);
if (node._bucket.Count > _bucketSize) {
node._val = 0.0;
foreach (PointDouble pt in node._bucket)
{
if (node.split == Node.Type.X)
node._val += pt.Y;
else
node._val += pt.X;
}
node._val /= node._bucket.Count;
node._left = new Node ();
node._right = new Node ();
foreach(PointDouble pt in node._bucket)
{
if (node.split == Node.Type.X)
if (pt.Y > node._val)
node._left._bucket.Add(pt);
else
node._right._bucket.Add (pt);
else
if (pt.X < node._val)
node._left._bucket.Add(pt);
else
node._right._bucket.Add (pt);
}
node._left.split = node._right.split = node.split == Node.Type.X ? Node.Type.Y : Node.Type.X;
//is this risky?
node._bucket.Clear ();
}
return;
}
switch (node.split)
{
case Node.Type.X:
if (point.Y > node._val){
addPoint(point, node._left);
}else{
addPoint(point, node._right);
}
break;
case Node.Type.Y:
if (point.X < node._val){
addPoint(point, node._left);
}else{
addPoint(point, node._right);
}
break;
}
}
public void addPoint(PointDouble point)
{
addPoint(point, _root);
}
public void GetPointsInRange(PointDouble point, double radsquare, Node node, List<PointDouble> listOfPoints)
{
if (node == null)
return;
//this is a leaf node
if (node._left == null && node._right == null)
{
foreach (PointDouble pt in node._bucket)
{
if( (pt.X - point.X)*(pt.X - point.X) +
(pt.Y - point.Y)*(pt.Y - point.Y) < radsquare)
{
listOfPoints.Add(pt);
}
}
return;
}
switch (node.split)
{
case Node.Type.X:
if (point.Y > node._val){
GetPointsInRange(point, radsquare, node._left, listOfPoints);
if ((point.Y - node._val)*(point.Y - node._val) <= radsquare)
GetPointsInRange(point, radsquare, node._right, listOfPoints);
}else{
GetPointsInRange(point, radsquare, node._right, listOfPoints);
if ((point.Y - node._val)*(point.Y - node._val) < radsquare)
GetPointsInRange(point, radsquare, node._left, listOfPoints);
}
break;
case Node.Type.Y:
if (point.X < node._val){
GetPointsInRange(point, radsquare, node._left, listOfPoints);
if ((point.X - node._val)*(point.X - node._val) <= radsquare)
GetPointsInRange(point, radsquare, node._right, listOfPoints);
}else{
GetPointsInRange(point, radsquare, node._right, listOfPoints);
if ((point.X - node._val)*(point.X - node._val) < radsquare)
GetPointsInRange(point, radsquare, node._left, listOfPoints);
}
break;
}
}
public List<PointDouble> GetPointsInRange(PointDouble point, double radius)
{
var points = new List<PointDouble>();
GetPointsInRange(point, radius*radius, _root, points);
return points;
}
public PointDouble GetClosestPoint(PointDouble point, Node node)
{
if (node == null)
return new PointDouble();
double distanceSqA = double.MaxValue;
double distanceSqB = double.MaxValue;
PointDouble closestPointA = null;
PointDouble closestPointB = null;
//this is a leaf node
if (node._left == null && node._right == null)
{
foreach (PointDouble pt in node._bucket)
{
if( (pt.X - point.X)*(pt.X - point.X) +
(pt.Y - point.Y)*(pt.Y - point.Y) < distanceSqA)
{
closestPointA = pt;
distanceSqA = (pt.X - point.X)*(pt.X - point.X) + (pt.Y - point.Y)*(pt.Y - point.Y);
}
}
return closestPointA;
}
switch (node.split)
{
case Node.Type.X:
if (point.Y > node._val){
closestPointA = GetClosestPoint(point, node._left);
}else{
closestPointA = GetClosestPoint(point, node._right);
}
break;
case Node.Type.Y:
if (point.X < node._val){
closestPointA = GetClosestPoint(point, node._left);
}else{
closestPointA = GetClosestPoint(point, node._right);
}
break;
}
if (closestPointA != null)
distanceSqA = (closestPointA.X - point.X)*(closestPointA.X - point.X) + (closestPointA.Y - point.Y)*(closestPointA.Y - point.Y);
switch (node.split)
{
case Node.Type.X:
if (point.Y > node._val){
if ((point.Y - node._val)*(point.Y - node._val) <= distanceSqA)
closestPointB = GetClosestPoint(point, node._right);
}else{
if ((point.Y - node._val)*(point.Y - node._val) < distanceSqA)
closestPointB = GetClosestPoint(point, node._left);
}
break;
case Node.Type.Y:
if (point.X < node._val){
if ((point.X - node._val)*(point.X - node._val) <= distanceSqA)
closestPointB = GetClosestPoint(point, node._right);
}else{
if ((point.X - node._val)*(point.X - node._val) < distanceSqA)
closestPointB = GetClosestPoint(point, node._left);
}
break;
}
if (closestPointB != null)
distanceSqB = (closestPointB.X - point.X)*(closestPointB.X - point.X) + (closestPointB.Y - point.Y)*(closestPointB.Y - point.Y);
return distanceSqA < distanceSqB ? closestPointA : closestPointB;
}
public PointDouble GetClosestPoint(PointDouble point)
{
return GetClosestPoint(point, _root);
}
