public static Ball GetExternalSphere(Box box)
{
var vectors = GetDirection(box);
var up = vectors[2];
Point3D center = box.GetAbsoluteLocation().ToPoint3D() + (0.5*up*box.ZSize);
double radius = Math.Sqrt(Math.Pow(0.5*box.ZSize, 2.0) + Math.Pow(0.5*box.YSize, 2.0) + Math.Pow(0.5*box.XSize, 2.0));
Point3D locaction = center - up.Normalize()*radius;
return new Ball(){Location = locaction.ToFrame(), Radius = radius};
}
public static Point3D[] GetDirection(Box box)
{
Matrix m = box.Location.GetMatrix();
var vectors = new Point3D[3];
vectors[0] = new Point3D(m[0, 0], m[1, 0], m[2, 0]);
vectors[1] = new Point3D(m[0, 1], m[1, 1], m[2, 1]);
vectors[2] = new Point3D(m[0, 2], m[1, 2], m[2, 2]);
//var up = vectors[2];
//var left = -vectors[1];
//var front = vectors[0];
return vectors;
}
public void Close()
{
Remove(Cap);
Cap = new Box
{
XSize = 18,
YSize = 4,
ZSize = 4,
DefaultColor = Cap.DefaultColor,
Location = new Frame3D(0, 0, 0),
IsStatic = true,
NewId = "CC",
};
Add(Cap);
}
public void TreeMovingNoParent()
{
var oldBoxLocation = new Frame3D(10, 20, 30);
var box = new Box
{
Location = oldBoxLocation
};
var newParent = new Body
{
Location = new Frame3D(20, 10, 20)
};
newParent.DetachAttachMaintaingLoction(box);
Assert.AreEqual(oldBoxLocation, box.GetAbsoluteLocation());
Assert.AreEqual(new Frame3D(-10, 10, 10), box.Location);
}
public void TreeMovingConstantLocation()
{
var world = new Body();
var boxLocation = new Frame3D(10, 0, 0);
var ballLocation = new Frame3D(20, 10, 0);
var box = new Box {Location = boxLocation};
var ball = new Ball {Location = ballLocation};
world.Add(box);
world.Add(ball);
box.DetachAttachMaintaingLoction(ball);
Assert.AreEqual(boxLocation, box.Location);
Assert.AreEqual(ballLocation, ball.GetAbsoluteLocation());
Assert.AreEqual(new Frame3D(10, 10, 0), ball.Location);
world.DetachAttachMaintaingLoction(ball);
Assert.AreEqual(ballLocation, ball.Location);
Assert.AreEqual(ballLocation, ball.GetAbsoluteLocation());
}
public void AbsoluteLocationLinear()
{
var root = new Body {Location = new Frame3D(0, 0, 10)};
Assert.AreEqual(root.Location, root.GetAbsoluteLocation());
var box = new Box {Location = new Frame3D(10, 0, 0)};
Assert.AreEqual(box.Location, box.GetAbsoluteLocation());
root.Add(box);
Assert.AreEqual(box.Location.Apply(root.Location), box.GetAbsoluteLocation());
int sum = 0;
for(int i = 0; i < 10; i++)
{
sum += i;
Body child = new Box {Location = new Frame3D(i, 0, 0)};
root.Add(child);
Assert.AreEqual(sum, child.GetAbsoluteLocation().X);
root = child;
}
}
public static double IntersectBox(Box box, Ray ray)
{
Ball ball = GetExternalSphere(box);
if (double.IsPositiveInfinity(IntersectBall(ball, ray)))
return double.PositiveInfinity;
else
{
var points = new List<Point3D>();
var vertex = GetVertex(box);
int[][] order = new int[12][];
order[0] = new[] {0, 2, 1};
order[1] = new[] {1, 2, 4};
order[2] = new[] {0, 5, 2};
order[3] = new[] {0, 3, 5};
order[4] = new[] {0, 3, 1};
order[5] = new[] {1, 3, 6};
order[6] = new[] {3, 5, 6};
order[7] = new[] {6, 5, 7};
order[8] = new[] {1, 6, 7};
order[9] = new[] {1, 7, 4};
order[10] = new[] {2, 5, 7};
order[11] = new[] {2, 7, 4};
for (int i = 0; i < 12; i++)
{
Point3D? point = TriangleIntersection.IntersectTriangle(vertex[order[i][0]], vertex[order[i][1]],
vertex[order[i][2]], ray);
if (point != null)
points.Add(point.Value);
else
{
point = TriangleIntersection.IntersectTriangle(vertex[order[i][0]], vertex[order[i][2]],
vertex[order[i][1]], ray);
if (point != null)
points.Add(point.Value);
}
}
if (points.Capacity == 0)
{
return double.PositiveInfinity;
}
return points.Min(a => ray.Origin.Hypot(a));
}
}
public void TestBoxes()
{
var box1 = new Box
{
DefaultColor = Color.Red
};
var box2 = new Box
{
Top = new SolidColorBrush {Color = Color.Red},
Bottom = new SolidColorBrush {Color = Color.Red},
Front = new SolidColorBrush {Color = Color.Red},
Back = new SolidColorBrush {Color = Color.Red},
Right = new SolidColorBrush {Color = Color.Red},
Left = new SolidColorBrush {Color = Color.Red}
};
Assert.That(_comparer.Equals(box1, box2));
box2.Front = new PlaneImageBrush {FilePath = "aax"};
Assert.That(!_comparer.Equals(box1, box2));
box2.Front = new SolidColorBrush {Color = box1.DefaultColor};
Assert.That(_comparer.Equals(box1, box2));
box2.Model = new Model();
Assert.That(!_comparer.Equals(box1, box2));
}
public static List<Point3D> GetVertex(Box box)
{
var location = box.GetAbsoluteLocation().ToPoint3D();
var x = box.XSize;
var y = box.YSize;
var z = box.ZSize;
Point3D[] vectors = GetDirection(box);
var nUp = vectors[2];
var nLeft = -vectors[1];
var nFront = vectors[0];
var vertex = new List<Point3D>
{
location + 0.5*nLeft*y - 0.5*nFront*x,
location + 0.5*nLeft*y + 0.5*nFront*x,
location - 0.5*nLeft*y - 0.5*nFront*x,
location + nUp*z + 0.5*nLeft*y - 0.5*nFront*x,
location - 0.5*nLeft*y + 0.5*nFront*x,
location + nUp*z - 0.5*nLeft*y - 0.5*nFront*x,
location + nUp*z + 0.5*nLeft*y + 0.5*nFront*x,
location + nUp*z - 0.5*nLeft*y + 0.5*nFront*x
};
return vertex;
}
public void Visit(Box visitable)
{
IPhysical physical = PhysicalManager.MakeBox(visitable.XSize, visitable.YSize, visitable.ZSize);
physical.Body = visitable;
AfterCreating(visitable, physical);
}
public Body CreateWorld(ICvarcEngine engine, ISceneSettings _settings)
{
Settings = (SceneSettings)_settings;
var root = new Body();
var first = new Cylinder
{
Height = 20,
RTop = 10,
RBottom = 10,
Location = new Frame3D(-150 + 25 - 10, 100 - 25 + 10, 3),
DefaultColor = Color.DarkViolet,
IsMaterial = true,
Density = Density.Iron,
FrictionCoefficient = 0,
Top = new PlaneImageBrush { Image = new Bitmap(GetResourceStream("red.png")) },
Type = "Robot"
};
var second = new Cylinder
{
Height = 20,
RTop = 10,
RBottom = 10,
Location = new Frame3D(150 - 25 + 10, 100 - 25 + 10, 3, Angle.Zero, Angle.Pi, Angle.Zero),
DefaultColor = Color.DarkViolet,
IsMaterial = true,
Density = Density.Iron,
FrictionCoefficient = 0,
Top = new PlaneImageBrush { Image = new Bitmap(GetResourceStream("blue.png")) },
Type = "Robot"
};
root.Add(first);
root.Add(second);
first.Collision += body => engine.RaiseOnCollision(first.Id.ToString(), body.Id.ToString(), CollisionType.RobotCollision);
second.Collision += body => engine.RaiseOnCollision(second.Id.ToString(), body.Id.ToString(), CollisionType.RobotCollision);
root.Add(new Box
{
XSize = 300,
YSize = 200,
ZSize = 3,
DefaultColor = Color.White,
Top = new SolidColorBrush { Color = Color.Yellow },
IsStatic = true,
Type = "floor",
});
foreach (var detail in Settings.Details)
{
Color color = Color.White;
string name = "D";
switch (detail.Color)
{
case DetailColor.Red: color = Color.Red; name += "R"; break;
case DetailColor.Blue: color = Color.Blue; name += "B"; break;
case DetailColor.Green: color = Color.Green; name += "G"; break;
}
var box = new Box
{
XSize = 15,
YSize = 15,
ZSize = 15,
Location = new Frame3D(-150 + 25 + detail.Location.X * 50, 100 - 25 - 50 * detail.Location.Y, 0),
DefaultColor = color,
Type = name,
IsMaterial = true,
IsStatic = false,
FrictionCoefficient = 8
};
root.Add(box);
box.Collision += body =>
{
if (box.Parent.Id == first.Id && body.Id == second.Id)
engine.RaiseOnCollision(second.Id.ToString(), first.Id.ToString(), CollisionType.RobotCollision);
if (box.Parent.Id == second.Id && body.Id == first.Id)
engine.RaiseOnCollision(first.Id.ToString(), second.Id.ToString(), CollisionType.RobotCollision);
};
}
CreateWalls(root, Settings.HorizontalWalls, 50, 10, 15, "HW", (x, y) => new Point(-150 + 25 + x * 50, 100 - (y + 1) * 50));
CreateWalls(root, Settings.VerticalWalls, 10, 50, 14, "VW", (x, y) => new Point(-150 + (x + 1) * 50, 100 - 25 - y * 50));
CreateBorders(root);
return root;
}
private void Release(ICvarcEngine engine, Body Body)
{
var latestGripped = Body.FirstOrDefault(z => z.Type.StartsWith("D") && z.Type.Length == 2);
if (latestGripped == null) return;
var absoluteLocation = latestGripped.GetAbsoluteLocation();
Body.Remove(latestGripped);
latestGripped.FrictionCoefficient = frictionCoefficientsById.SafeGet(latestGripped.Id);
var targetColor = latestGripped.Type[1].ToString();
latestGripped.Location = absoluteLocation;
latestGripped.Velocity = Body.Velocity;
var toAtt = Body.TreeRoot.GetSubtreeChildrenFirst()
.Where(a =>
(a.Type == "VW" + targetColor || a.Type == "HW" + targetColor) &&
Distance(latestGripped, a) < 30)
.OfType<Box>()
.FirstOrDefault();
if (toAtt != null)
{
Body.TreeRoot.Remove(toAtt);
var wall = new Box
{
Type = toAtt.Type.Substring(0, 2),
XSize = toAtt.XSize,
YSize = toAtt.YSize,
ZSize = toAtt.ZSize,
Location = toAtt.Location,
DefaultColor = SRCompetitions.WallColor,
IsStatic = true,
IsMaterial = true
};
Body.TreeRoot.Add(wall);
engine.RaiseOnCollision(Body.Id.ToString(), toAtt.Id.ToString(), GetCollisionType(targetColor));
}
else
Body.TreeRoot.Add(latestGripped);
// gripped.RemoveRange(0, gripped.Count);
}
public void BoxTest(Box box)
{
var v = Intersector.GetDirection(box);
for (int i = 0; i < 3; i++)
{
Console.WriteLine(v[i]);
}
var vertex = Intersector.GetVertex(box);
for (int i = 0; i < vertex.Capacity; i++)
{
Console.WriteLine("next: " + vertex[i]);
}
}
public void RayTest(Ray ray, Box box, double distance)
{
var computedDistance = Intersector.IntersectBox(box, ray);
Assert.AreEqual(distance, computedDistance);
{
Console.WriteLine("Distance: {0}. expected: {1}", computedDistance, distance);
}
}
