private DynamicTreeTest()
{
_worldExtent = 15.0f;
_proxyExtent = 0.5f;
Rand.Random = new Random(888);
for (int i = 0; i < ActorCount; ++i)
{
_actors[i] = new Actor();
Actor actor = _actors[i];
GetRandomAABB(out actor.AABB);
actor.ProxyId = _tree.AddProxy(ref actor.AABB, actor);
}
float h = _worldExtent;
_queryAABB.LowerBound = new Vector2(-3.0f, -4.0f + h);
_queryAABB.UpperBound = new Vector2(5.0f, 6.0f + h);
_rayCastInput.Point1 = new Vector2(-5.0f, 5.0f + h);
_rayCastInput.Point2 = new Vector2(7.0f, -4.0f + h);
//_rayCastInput.p1 = new Vector2(0.0f, 2.0f + h);
//_rayCastInput.p2 = new Vector2(0.0f, -2.0f + h);
_rayCastInput.MaxFraction = 1.0f;
_automated = false;
}
public void TestManipulation()
{
var tree = new DynamicTree();
var aabb1 = new AABB(new Vector2D(21.9995f, 1.4995f), new Vector2D(28.0005f, 2.2005f));
var proxy1 = tree.AddProxy(aabb1, null);
Assert.AreEqual(1, tree.Count);
Assert.AreEqual(0, proxy1.Depth);
Assert.IsTrue(proxy1.IsLeaf);
Assert.IsTrue(proxy1.IsRoot);
var aabb2 = new AABB(new Vector2D(21.9995f, 1.9995f), new Vector2D(22.5005f, 3.7005f));
var proxy2 = tree.AddProxy(aabb2, null);
Assert.AreEqual(3, tree.Count);
Assert.AreEqual(0, proxy2.Depth);
Assert.IsTrue(proxy2.IsLeaf);
Assert.IsFalse(proxy2.IsRoot);
var aabb3 = new AABB(new Vector2D(27.4995f, 4.9995f), new Vector2D(28.0005f, 6.7005f));
var proxy3 = tree.AddProxy(aabb3, null);
Assert.AreEqual(5, tree.Count);
Assert.AreEqual(0, proxy3.Depth);
Assert.IsTrue(proxy3.IsLeaf);
Assert.IsFalse(proxy3.IsRoot);
var aabb4 = new AABB(new Vector2D(23.4495f, 1.1495f), new Vector2D(24.9505f, 3.8505f));
var proxy4 = tree.AddProxy(aabb4, null);
Assert.AreEqual(7, tree.Count);
Assert.AreEqual(0, proxy4.Depth);
Assert.IsTrue(proxy4.IsLeaf);
Assert.IsFalse(proxy4.IsRoot);
tree.RemoveProxy(proxy2);
Assert.AreEqual(5, tree.Count);
Assert.AreEqual(2, tree.FreeCount);
Assert.AreEqual(-1, proxy2.Depth);
Assert.IsTrue(proxy2.IsFree);
aabb4 = new AABB(new Vector2D(23.8495f, 5.9495f), new Vector2D(25.3505f, 8.6505f));
tree.MoveProxy(proxy4, aabb4, new Vector2D(1, 1));
Assert.AreEqual(5, tree.Count);
Assert.AreEqual(2, tree.FreeCount);
}
private static DynamicTree CreateTestTree()
{
var tree = new DynamicTree();
var aabb1 = new AABB(new Vector2D(21.9995f, 1.4995f), new Vector2D(28.0005f, 2.2005f));
var proxy1 = tree.AddProxy(aabb1, 1);
var aabb2 = new AABB(new Vector2D(21.9995f, 1.9995f), new Vector2D(22.5005f, 3.7005f));
var proxy2 = tree.AddProxy(aabb2, 2);
var aabb3 = new AABB(new Vector2D(27.4995f, 4.9995f), new Vector2D(28.0005f, 6.7005f));
var proxy3 = tree.AddProxy(aabb3, 3);
var aabb4 = new AABB(new Vector2D(23.8495f, 5.9495f), new Vector2D(25.3505f, 8.6505f));
var proxy4 = tree.AddProxy(aabb4, 4);
var aabb5 = new AABB(new Vector2D(25.4995f, 6.4995f), new Vector2D(28.0005f, 7.2005f));
var proxy5 = tree.AddProxy(aabb5, 5);
return(tree);
}
private void AddPointsAndSprings(List <TriangleVertexIndices> indices, List <JVector> vertices)
{
for (int i = 0; i < vertices.Count; i++)
{
MassPoint point = new MassPoint(sphere, this, material);
point.Position = vertices[i];
point.Mass = 0.1f;
points.Add(point);
}
for (int i = 0; i < indices.Count; i++)
{
TriangleVertexIndices index = indices[i];
Triangle t = new Triangle(this);
t.indices = index;
triangles.Add(t);
t.boundingBox = JBBox.SmallBox;
t.boundingBox.AddPoint(points[t.indices.I0].position);
t.boundingBox.AddPoint(points[t.indices.I1].position);
t.boundingBox.AddPoint(points[t.indices.I2].position);
t.dynamicTreeID = dynamicTree.AddProxy(ref t.boundingBox, t);
}
HashSet <Edge> edges = GetEdges(indices);
int count = 0;
foreach (Edge edge in edges)
{
Spring spring = new Spring(points[edge.Index1], points[edge.Index2]);
spring.Softness = 0.01f; spring.BiasFactor = 0.1f;
spring.SpringType = SpringType.EdgeSpring;
springs.Add(spring);
count++;
}
}
public void Init()
{
_hasInited = false;
EventGroup = new EventGroup();
MapConfig = Logic.Map.Map.Deserialize(Name);
FixedQuadTree = new FixedQuadTree <SceneObject>();
FixedQuadTree.SetBounds(new Utility.FixedRect(-FixedMath.One * MapConfig.Width / 2, -FixedMath.One * MapConfig.Height / 2, FixedMath.One * MapConfig.Width, FixedMath.One * MapConfig.Height));
PhysicsTree = new DynamicTree <FixtureProxy>();
for (int i = 0; i < MapConfig.Data.Data.Count; i++)
{
var stageData = MapConfig.Data.Data[i];
var aabb = new AABB(new Vector2d(stageData.X * FixedMath.One + FixedMath.Half, stageData.Y * FixedMath.One + FixedMath.Half), FixedMath.One, FixedMath.One);
var fp = new FixtureProxy();
fp.AABB = aabb;
fp.Fixture = new Transform2d()
{
p = aabb.Center, angle = 0
};
var nodeid = PhysicsTree.AddProxy(ref aabb, fp);
}
EventGroup.ListenEvent(SceneEvent.OnLoaded.ToInt(), OnLoded);
}
// These support body activation/deactivation.
internal void CreateProxies(DynamicTree <FixtureProxy> fixtureTree, ref Transform xf)
{
if (ProxyCount != 0)
{
throw new InvalidOperationException("Proxies allready created for this Fixture.");
}
// Create proxies in the broad-phase.
ProxyCount = Shape.ChildCount;
for (int i = 0; i < ProxyCount; ++i)
{
FixtureProxy proxy = new FixtureProxy();
proxy.Fixture = this;
proxy.ChildIndex = i;
Shape.ComputeAABB(out proxy.AABB, ref xf, i);
//FPE note: This line needs to be after the previous two because FixtureProxy is a struct
proxy.ProxyId = fixtureTree.AddProxy(ref proxy.AABB, proxy);
//broadPhase.SetProxy(proxy.ProxyId, ref proxy);
Proxies[i] = proxy;
}
}
// Use this for initialization
void Start()
{
return;
func = callBack;
//int count = 0;
//for (int i = 0; i < 100; i++)
//{
// var g = GameObject.CreatePrimitive(PrimitiveType.Cube);
// g.transform.position = new Vector3(Random.Range(0, 100), 0, Random.Range(0, 100));
//}
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
//sw.Start();
//for (int i = 0; i < 100; i++)
//{
// Ray r = new Ray();
// r.origin = new Vector3(Random.Range(0, 50),0, Random.Range(0, 50));
// r.direction = new Vector3(Random.Range(0, 1f), 0, Random.Range(0, 1f));
// r.direction = r.direction.normalized;
// var hit = Physics.Raycast(r, 100, -1);
// // Debug.DrawLine(r.origin, r.direction*100+r.origin, Color.red, 10);
// if (hit) count++;
//}
// Debug.Log(sw.ElapsedMilliseconds);
//return;
Vector2d[] vs = new Vector2d[4];
for (int i = 0; i < 100; i++)
{
var center = new Vector2d(Random.Range(0, 100), Random.Range(0, 100));
var aabb = new AABB(center, FixedMath.One * 2, FixedMath.One * 2);
FixtureProxy fp = new FixtureProxy();
long angle = FixedMath.One * (int)(Random.Range(0, 359));
Vector2d a = -aabb.Extents;
Vector2d b = a + new Vector2d(0, aabb.Height);
Vector2d c = aabb.Extents;
Vector2d d = a + new Vector2d(aabb.Width, 0);
var halfw = aabb.Width / 2;
var halfh = aabb.Height / 2;
var radius = FixedMath.Sqrt((halfw).Mul(halfw) + halfh.Mul(halfh));
a = RotatePosi(a, angle);
b = RotatePosi(b, angle);
c = RotatePosi(c, angle);
d = RotatePosi(d, angle);
vs[0] = a;
vs[1] = b;
vs[2] = c;
vs[3] = d;
Vector2d min = Vector2d.Min(vs) + center;
Vector2d max = Vector2d.Max(vs) + center;
DLog.Log(radius.ToFloat().ToString());
var outteraabb = new AABB(center, radius * 2, radius * 2);
fp.AABB = aabb;
fp.Fixture = new Transform2d(ref center, ref angle);
int id = tree.AddProxy(ref outteraabb, fp);
tree.MoveProxy(id, ref outteraabb, Vector2d.zero);
DrawFixtureProxy(fp);
DrawAABB(outteraabb);
}
//var bcs = GameObject.FindObjectsOfType<BoxCollider>();
//for (int i = 0; i < bcs.Length; i++)
//{
// var bc = bcs[i];
// var aabb = new AABB(new Vector2d(bc.transform.position), FixedMath.One, FixedMath.One);
// FixtureProxy fp = new FixtureProxy();
// fp.AABB = aabb;
// long angle = FixedMath.One * (int)(bc.transform.eulerAngles.y);
// Vector2d p = new Vector2d(bc.transform.position);
// fp.Fixture = new Transform2d(ref p, ref angle);
// tree.AddProxy(ref aabb, fp);
// DrawFixtureProxy(fp);
//}
// sw.Reset();
sw.Start();
for (int i = 0; i < 1; i++)
{
RayCastInput input = new RayCastInput();
input.Point1 = new Vector2d(Random.Range(0, 50) * FixedMath.One, Random.Range(0, 50) * FixedMath.One);
input.Point2 = new Vector2d(Random.Range(50, 100) * FixedMath.One, Random.Range(50, 100) * FixedMath.One);
NewSphere(input.Point1.ToVector3(), "start");
NewSphere(input.Point2.ToVector3(), "end");
input.MaxFraction = FixedMath.One;
DrawLine(input);
tree.RayCast(callBack, ref input);
}
Debug.Log(sw.ElapsedMilliseconds);
}
