public virtual void Update(float timestep)
{
active = false;
foreach (MassPoint point in points)
{
if (point.isActive && !point.isStatic)
{
active = true; break;
}
}
if (!active)
{
return;
}
box = JBBox.SmallBox;
volume = 0.0f;
mass = 0.0f;
foreach (MassPoint point in points)
{
mass += point.Mass;
box.AddPoint(point.position);
}
box.Min -= new JVector(TriangleExpansion);
box.Max += new JVector(TriangleExpansion);
foreach (Triangle t in triangles)
{
// Update bounding box and move proxy in dynamic tree.
JVector prevCenter = t.boundingBox.Center;
t.UpdateBoundingBox();
JVector linVel = t.VertexBody1.linearVelocity +
t.VertexBody2.linearVelocity +
t.VertexBody3.linearVelocity;
linVel *= 1.0f / 3.0f;
dynamicTree.MoveProxy(t.dynamicTreeID, ref t.boundingBox, linVel * timestep);
JVector v1 = points[t.indices.I0].position;
JVector v2 = points[t.indices.I1].position;
JVector v3 = points[t.indices.I2].position;
volume -= ((v2.Y - v1.Y) * (v3.Z - v1.Z) -
(v2.Z - v1.Z) * (v3.Y - v1.Y)) * (v1.X + v2.X + v3.X);
}
volume /= 6.0f;
AddPressureForces(timestep);
}
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 void MoveProxy()
{
for (int i = 0; i < ActorCount; ++i)
{
int j = Rand.Random.Next() % ActorCount;
Actor actor = _actors[j];
if (actor.ProxyId == -1)
{
continue;
}
AABB aabb0 = actor.AABB;
MoveAABB(ref actor.AABB);
Vector2 displacement = actor.AABB.Center - aabb0.Center;
_tree.MoveProxy(actor.ProxyId, ref actor.AABB, displacement);
return;
}
}
private void MoveProxy()
{
for (int i = 0; i < _actorCount; ++i)
{
int j = _rand.Next() % _actorCount;
Actor actor = _actors[j];
if (actor.proxyId == DynamicTree <Actor> .NullNode)
{
continue;
}
AABB aabb0 = actor.aabb;
MoveAABB(ref actor.aabb);
Vector2 displacement = actor.aabb.Center - aabb0.Center;
_tree.MoveProxy(actor.proxyId, ref actor.aabb, displacement);
return;
}
}
private void MoveProxy()
{
for (int i = 0; i < e_actorCount; ++i)
{
int j = Rand.rand.Next() % e_actorCount;
Actor actor = _actors[j];
if (actor.proxyId == -1)
{
continue;
}
AABB aabb0 = actor.aabb;
MoveAABB(ref actor.aabb);
Vector2 displacement = actor.aabb.GetCenter() - aabb0.GetCenter();
_tree.MoveProxy(actor.proxyId, ref actor.aabb, displacement);
return;
}
}
// 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);
}