public World(AABB worldAABB, Vec2 gravity, bool doSleep)
{
this._destructionListener = null;
this._boundaryListener = null;
this._contactFilter = WorldCallback.DefaultFilter;
this._contactListener = null;
this._debugDraw = null;
this._bodyList = null;
this._contactList = null;
this._jointList = null;
this._bodyCount = 0;
this._contactCount = 0;
this._jointCount = 0;
this._warmStarting = true;
this._continuousPhysics = true;
this._allowSleep = doSleep;
this._gravity = gravity;
this._lock = false;
this._inv_dt0 = 0f;
this._contactManager = new ContactManager();
this._contactManager._world = this;
this._broadPhase = new BroadPhase(worldAABB, this._contactManager);
BodyDef def = new BodyDef();
this._groundBody = this.CreateBody(def);
}
protected override void Initialize()
{
AABB aabb = new AABB();
aabb.LowerBound = new Vec2(-10000, -10000);
aabb.UpperBound = new Vec2(10000, 10000);
world = new World(aabb, new Vec2(0, 0), false);
basicEffect = new BasicEffect(this.GraphicsDevice);
basicEffect.DiffuseColor = new Vector3(1, 1, 1);
Components.Add(new Map(this, "MainMap"));
CarData cd = Json.GetCar("BMW");
car = new Car(this, cd.Position, cd.CollisionBody, cd.ModelName, cd.Density, cd.Friction, cd.Scale, cd.Angle);
Components.Add(car);
camera = new Camera(car, this);
Font = Content.Load<SpriteFont>("SpriteFont1");
base.Initialize();
}
public override void ComputeAABB(out AABB aabb, XForm transform)
{
aabb = default(AABB);
Vec2 vec = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition);
aabb.LowerBound.Set(vec.X - this._radius, vec.Y - this._radius);
aabb.UpperBound.Set(vec.X + this._radius, vec.Y + this._radius);
}
public override void ComputeAABB(out AABB aabb, Transform xf)
{
aabb = new AABB();
Vector2 p = xf.position + xf.TransformDirection(_position);
aabb.LowerBound = new Vector2(p.x - _radius, p.y - _radius);
aabb.UpperBound = new Vector2(p.x + _radius, p.y + _radius);
}
public override void ComputeAABB(out AABB aabb, ref Transform transform)
{
aabb = new AABB();
Vec2 p = transform.Position + Math.Mul(transform.R, _p);
aabb.LowerBound.Set(p.X - _radius, p.Y - _radius);
aabb.UpperBound.Set(p.X + _radius, p.Y + _radius);
}
public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2)
{
aabb = default(AABB);
Vec2 a = transform1.Position + Box2DX.Common.Math.Mul(transform1.R, this._localPosition);
Vec2 b = transform2.Position + Box2DX.Common.Math.Mul(transform2.R, this._localPosition);
Vec2 vec = Box2DX.Common.Math.Min(a, b);
Vec2 vec2 = Box2DX.Common.Math.Max(a, b);
aabb.LowerBound.Set(vec.X - this._radius, vec.Y - this._radius);
aabb.UpperBound.Set(vec2.X + this._radius, vec2.Y + this._radius);
}
public static void DrawAABB(AABB aabb, Color c)
{
Gl.Color3(c.R, c.G, c.B);
Gl.Begin(BeginMode.LineLoop);
Gl.Vertex2(aabb.LowerBound.X, aabb.LowerBound.Y);
Gl.Vertex2(aabb.UpperBound.X, aabb.LowerBound.Y);
Gl.Vertex2(aabb.UpperBound.X, aabb.UpperBound.Y);
Gl.Vertex2(aabb.LowerBound.X, aabb.UpperBound.Y);
Gl.End();
}
public static void DrawAABB(AABB aabb, Color c)
{
Gl.glColor3f(c.R, c.G, c.B);
Gl.glBegin(Gl.GL_LINE_LOOP);
Gl.glVertex2f(aabb.LowerBound.X, aabb.LowerBound.Y);
Gl.glVertex2f(aabb.UpperBound.X, aabb.LowerBound.Y);
Gl.glVertex2f(aabb.UpperBound.X, aabb.UpperBound.Y);
Gl.glVertex2f(aabb.LowerBound.X, aabb.UpperBound.Y);
Gl.glEnd();
}
public CASSWorld(float width, float height, Vector2 gravity)
{
// Create the world's axis-aligned bounding box
AABB aabb = new AABB();
aabb.LowerBound = new Vec2(-MARGIN, -MARGIN);
aabb.UpperBound = new Vec2(width + MARGIN, height + MARGIN);
world = new World(aabb, Utils.Convert(gravity), true);
succeeded = failed = false;
}
public Test()
{
AABB aabb = new AABB();
aabb.UpperBound = new UnityEngine.Vector2(100,100);
aabb.LowerBound = new UnityEngine.Vector2(-100,-100);
World world = new World(aabb, new Vector2(0, -1), false);
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(0, 0);
PolygonShape groundBox = new PolygonShape();
groundBox.SetAsBox(10, 1);
Body body = new Body(groundBodyDef, world);
}
public InGameState()
{
//position the score display centered at the top
ScoreDisplay.CharacterSize = 90;
ScoreDisplay.DisplayedString = InGameState.WinCount0.ToString();
float leftSize = ScoreDisplay.GetLocalBounds().Width;
ScoreDisplay.DisplayedString += " : ";
float midSize = ScoreDisplay.GetLocalBounds().Width - leftSize;
ScoreDisplay.DisplayedString += InGameState.WinCount1.ToString();
ScoreDisplay.Origin = new Vector2(leftSize + 0.5f * midSize, 0f);
ScoreDisplay.Position = new Vector2(Constants.windowSizeX / 2 , Constants.windowSizeY / 14);
ScoreDisplay.Color = new SFML.Graphics.Color(200, 255, 200);
AABB aabb = new AABB();
aabb.LowerBound.Set(0.0f, 0.0f);
aabb.UpperBound.Set(800, 600/*Constants.worldSizeX * Constants.screenRatio*/);
physicsWorld = new World(aabb, new Vec2(0.0f, -9.81f), false);
contactManager = Physics.ContactManager.g_contactManager;
physicsWorld.SetContactListener(contactManager);
// Set new Players and appending dekoHands
ResetPlayers();
setDekoFlags();
//0xF0A58A4
groundPolygonAct = new Actors.PolygonActor(physicsWorld, new Vec2(0.0f, 15.0f), 0xFBA58A4, Actors.FunctionType.GradientNoise, 4);
BackgroundBackSprite = new Sprite(AssetManager.getTexture(AssetManager.TextureName.InGameBackGroundBack));
BackgroundBackSprite.Scale = new Vector2(Constants.windowSizeX / (float)BackgroundBackSprite.TextureRect.Width, Constants.windowSizeY / (float)BackgroundBackSprite.TextureRect.Height);//0.5F * Vector2.One;
BackgroundFrontSprite = new Sprite(AssetManager.getTexture(AssetManager.TextureName.InGameBackGroundFront));
BackgroundFrontSprite.Scale = BackgroundBackSprite.Scale;
//left and right borders of the map
BodyDef bodydef = new BodyDef();
bodydef.Position = new Vector2(0,0);
bodydef.Angle = 0.0f;
PolygonDef box = new PolygonDef();
box.SetAsBox(1f, Constants.worldSizeY);
Body leftEdge = physicsWorld.CreateBody(bodydef);
contactManager.addNonLethalShape(leftEdge.CreateShape(box));
bodydef.Position = new Vector2(Constants.worldSizeX-1, 0);
Body rightEdge = physicsWorld.CreateBody(bodydef);
contactManager.addNonLethalShape(rightEdge.CreateShape(box));
bodydef.Position = new Vector2(0, Constants.worldSizeY);
box.SetAsBox(Constants.worldSizeX, 1f);
Body topEdge = physicsWorld.CreateBody(bodydef);
contactManager.addNonLethalShape(topEdge.CreateShape(box));
}
public static bool TestOverlap(AABB a, AABB b)
{
Vec2 d1, d2;
d1 = b.LowerBound - a.UpperBound;
d2 = a.LowerBound - b.UpperBound;
if (d1.X > 0.0f || d1.Y > 0.0f)
return false;
if (d2.X > 0.0f || d2.Y > 0.0f)
return false;
return true;
}
public BroadPhase(AABB worldAABB, PairCallback callback)
{
this._pairManager = new PairManager();
this._pairManager.Initialize(this, callback);
Box2DXDebug.Assert(worldAABB.IsValid);
this._worldAABB = worldAABB;
this._proxyCount = 0;
Vec2 vec = worldAABB.UpperBound - worldAABB.LowerBound;
this._quantizationFactor.X = (float)BroadPhase.BROADPHASE_MAX / vec.X;
this._quantizationFactor.Y = (float)BroadPhase.BROADPHASE_MAX / vec.Y;
ushort num = 0;
while ((int)num < Settings.MaxProxies - 1)
{
this._proxyPool[(int)num] = new Proxy();
this._proxyPool[(int)num].Next = (ushort)(num + 1);
this._proxyPool[(int)num].TimeStamp = 0;
this._proxyPool[(int)num].OverlapCount = BroadPhase.Invalid;
this._proxyPool[(int)num].UserData = null;
num += 1;
}
this._proxyPool[Settings.MaxProxies - 1] = new Proxy();
this._proxyPool[Settings.MaxProxies - 1].Next = PairManager.NullProxy;
this._proxyPool[Settings.MaxProxies - 1].TimeStamp = 0;
this._proxyPool[Settings.MaxProxies - 1].OverlapCount = BroadPhase.Invalid;
this._proxyPool[Settings.MaxProxies - 1].UserData = null;
this._freeProxy = 0;
this._timeStamp = 1;
this._queryResultCount = 0;
for (int i = 0; i < 2; i++)
{
this._bounds[i] = new Bound[2 * Settings.MaxProxies];
}
int num2 = 2 * Settings.MaxProxies;
for (int j = 0; j < 2; j++)
{
for (int k = 0; k < num2; k++)
{
this._bounds[j][k] = new Bound();
}
}
}
public BroadPhaseTest()
{
BroadPhase.IsValidate = true;
//srand(888);
AABB worldAABB = new AABB();
worldAABB.LowerBound.Set(-5.0f * k_extent, -5.0f * k_extent);
worldAABB.UpperBound.Set(5.0f * k_extent, 5.0f * k_extent);
_overlapCount = 0;
_overlapCountExact = 0;
_callback._test = this;
_broadPhase = new BroadPhase(worldAABB, _callback);
for (int i = 0; i < k_actorCount; i++)
_overlaps[i] = new bool[k_actorCount];
for (int i = 0; i < k_actorCount; i++)
for (int j = 0; j < k_actorCount; j++)
_overlaps[i][j] = false;
for (int i = 0; i < k_actorCount; i++)
_actors[i] = new Actor();
for (int i = 0; i < k_actorCount; ++i)
{
bool s = false;
if (i == 91)
s = true;
Actor actor = _actors[i];
GetRandomAABB(ref actor.aabb);
//actor->aabb.minVertex.Set(0.0f, 0.0f);
//actor->aabb.maxVertex.Set(k_width, k_width);
actor.proxyId = _broadPhase.CreateProxy(actor.aabb, actor);
actor.overlapCount = 0;
_broadPhase.Validate();
}
_automated = true;
_stepCount = 0;
}
private void ComputeBounds(out ushort[] lowerValues, out ushort[] upperValues, AABB aabb)
{
lowerValues = new ushort[2];
upperValues = new ushort[2];
Box2DXDebug.Assert(aabb.UpperBound.X >= aabb.LowerBound.X);
Box2DXDebug.Assert(aabb.UpperBound.Y >= aabb.LowerBound.Y);
Vec2 vec = Box2DX.Common.Math.Clamp(aabb.LowerBound, this._worldAABB.LowerBound, this._worldAABB.UpperBound);
Vec2 vec2 = Box2DX.Common.Math.Clamp(aabb.UpperBound, this._worldAABB.LowerBound, this._worldAABB.UpperBound);
lowerValues[0] = (ushort)((ushort)(this._quantizationFactor.X * (vec.X - this._worldAABB.LowerBound.X)) & BroadPhase.BROADPHASE_MAX - 1);
upperValues[0] = (ushort)((ushort)(this._quantizationFactor.X * (vec2.X - this._worldAABB.LowerBound.X)) | 1);
lowerValues[1] = (ushort)((ushort)(this._quantizationFactor.Y * (vec.Y - this._worldAABB.LowerBound.Y)) & BroadPhase.BROADPHASE_MAX - 1);
upperValues[1] = (ushort)((ushort)(this._quantizationFactor.Y * (vec2.Y - this._worldAABB.LowerBound.Y)) | 1);
}
public bool InRange(AABB aabb)
{
Vec2 vec = Box2DX.Common.Math.Max(aabb.LowerBound - this._worldAABB.UpperBound, this._worldAABB.LowerBound - aabb.UpperBound);
return Box2DX.Common.Math.Max(vec.X, vec.Y) < 0f;
}
public abstract void ComputeSweptAABB(out AABB aabb, XForm xf1, XForm xf2);
// Call MoveProxy as many times as you like, then when you are done
// call Commit to finalized the proxy pairs (for your time step).
public void MoveProxy(int proxyId, AABB aabb)
{
if (proxyId == PairManager.NullProxy || Settings.MaxProxies <= proxyId)
{
Box2DXDebug.Assert(false);
return;
}
if (aabb.IsValid == false)
{
Box2DXDebug.Assert(false);
return;
}
int boundCount = 2 * _proxyCount;
Proxy proxy = _proxyPool[proxyId];
// Get new bound values
BoundValues newValues = new BoundValues();;
ComputeBounds(out newValues.LowerValues, out newValues.UpperValues, aabb);
// Get old bound values
BoundValues oldValues = new BoundValues();
for (int axis = 0; axis < 2; ++axis)
{
oldValues.LowerValues[axis] = _bounds[axis][proxy.LowerBounds[axis]].Value;
oldValues.UpperValues[axis] = _bounds[axis][proxy.UpperBounds[axis]].Value;
}
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex = proxy.LowerBounds[axis];
int upperIndex = proxy.UpperBounds[axis];
ushort lowerValue = newValues.LowerValues[axis];
ushort upperValue = newValues.UpperValues[axis];
int deltaLower = lowerValue - bounds[lowerIndex].Value;
int deltaUpper = upperValue - bounds[upperIndex].Value;
bounds[lowerIndex].Value = lowerValue;
bounds[upperIndex].Value = upperValue;
//
// Expanding adds overlaps
//
// Should we move the lower bound down?
if (deltaLower < 0)
{
int index = lowerIndex;
while (index > 0 && lowerValue < bounds[index - 1].Value)
{
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.ProxyId;
Proxy prevProxy = _proxyPool[prevBound.ProxyId];
++prevBound.StabbingCount;
if (prevBound.IsUpper == true)
{
if (TestOverlap(newValues, prevProxy))
{
_pairManager.AddBufferedPair(proxyId, prevProxyId);
}
++prevProxy.UpperBounds[axis];
++bound.StabbingCount;
}
else
{
++prevProxy.LowerBounds[axis];
--bound.StabbingCount;
}
--proxy.LowerBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index - 1]);
--index;
}
}
// Should we move the upper bound up?
if (deltaUpper > 0)
{
int index = upperIndex;
while (index < boundCount - 1 && bounds[index + 1].Value <= upperValue)
{
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.ProxyId;
Proxy nextProxy = _proxyPool[nextProxyId];
++nextBound.StabbingCount;
if (nextBound.IsLower == true)
{
if (TestOverlap(newValues, nextProxy))
{
_pairManager.AddBufferedPair(proxyId, nextProxyId);
}
--nextProxy.LowerBounds[axis];
++bound.StabbingCount;
}
else
{
--nextProxy.UpperBounds[axis];
--bound.StabbingCount;
}
++proxy.UpperBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index + 1]);
++index;
}
}
//
// Shrinking removes overlaps
//
// Should we move the lower bound up?
if (deltaLower > 0)
{
int index = lowerIndex;
while (index < boundCount - 1 && bounds[index + 1].Value <= lowerValue)
{
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.ProxyId;
Proxy nextProxy = _proxyPool[nextProxyId];
--nextBound.StabbingCount;
if (nextBound.IsUpper)
{
if (TestOverlap(oldValues, nextProxy))
{
_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
}
--nextProxy.UpperBounds[axis];
--bound.StabbingCount;
}
else
{
--nextProxy.LowerBounds[axis];
++bound.StabbingCount;
}
++proxy.LowerBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index + 1]);
++index;
}
}
// Should we move the upper bound down?
if (deltaUpper < 0)
{
int index = upperIndex;
while (index > 0 && upperValue < bounds[index - 1].Value)
{
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.ProxyId;
Proxy prevProxy = _proxyPool[prevProxyId];
--prevBound.StabbingCount;
if (prevBound.IsLower == true)
{
if (TestOverlap(oldValues, prevProxy))
{
_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
}
++prevProxy.LowerBounds[axis];
--bound.StabbingCount;
}
else
{
++prevProxy.UpperBounds[axis];
++bound.StabbingCount;
}
--proxy.UpperBounds[axis];
Common.Math.Swap <Bound>(ref bounds[index], ref bounds[index - 1]);
--index;
}
}
}
if (IsValidate)
{
Validate();
}
}
/// Combine two AABBs into this one.
public void Combine(AABB aabb1, AABB aabb2)
{
LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound);
UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound);
}
/// <summary>
/// Allows the game to perform any initialization it needs to before starting to run.
/// This is where it can query for any required services and load any non-graphic
/// related content. Calling base.Initialize will enumerate through any components
/// and initialize them as well.
/// </summary>
protected override void Initialize()
{
// TODO: Add your initialization logic here
IsMouseVisible = true;
#region phys
//world bounds
// if bodies reach the end of the world, but it will be slower or stops completly.
Box2DX.Collision.AABB worldAABB = new Box2DX.Collision.AABB();
worldAABB.LowerBound.Set(-100.0f, -100.0f);
worldAABB.UpperBound.Set(100.0f, 100.0f);
physicsWorld = new Engine.Physics.PhysicsWorld(new Vector2(0, 9.8f).ToBox2DVector(), worldAABB);
// Define the ground body.
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(0.0f / physicsScaleX, 256.0f / physicsScaleY);
// Call the body factory which creates the ground box shape.
// The body is also added to the world.
Body groundBody = physicsWorld.CreateBody(groundBodyDef);
// Define the ground box shape.
PolygonDef groundShapeDef = new PolygonDef();
// The extents are the half-widths of the box.
groundShapeDef.SetAsBox(1000.0f / physicsScaleX, 10.0f / physicsScaleY);
// Add the ground shape to the ground body.
groundBody.CreateShape(groundShapeDef);
#endregion
//FMOD.VERSION.dll
base.Initialize();
System.Diagnostics.Debug.WriteLine(System.IO.Path.GetFullPath("FMOD/64/fmod.dll"));
if (System.Environment.Is64BitProcess)
{
LoadLibrary(System.IO.Path.GetFullPath("FMOD/64/fmod.dll"));
}
else
{
LoadLibrary(System.IO.Path.GetFullPath("FMOD/32/fmod.dll"));
}
truck = Content.Load <Model>("Models/truck");
soundPlayer = new Engine.Sound.SoundPlayer(this);
soundPlayer.Init();
//generate and init landscape
foreach (var actor in GenerateLandscape(50, 50, 1, 1))
{
AddActor(actor); actor.Init();
}
mouseDisplayActor = new Engine.Actor(this, "mousedisplay", new Vector3(0, 0, 0), new Vector3(0, 0, 0), 0.0f);
mouseDisplayActor.Components.Add(new Engine.Components.ImageDisplayComponent(this, mouseDisplayActor, "Textures/mouse/icons8-cursor-24"));
mouseDisplayActor.Components.Add(new Engine.Components.MouseFollowComponent(this, mouseDisplayActor));
mouseDisplayActor.Init();
AddActor(new Actor(this, "ConcertHallReverb", new Vector3(0, 0, 0), new Vector3(0, 0, 0), 0.0f));
GetActorByName("ConcertHallReverb").Components.Add(new Engine.Components.ReverbComponent(this, GetActorByName("ConcertHallReverb"), new Vector3(0, 0, 0), FMOD.PRESET.CONCERTHALL(), 32, 32));
GetActorByName("ConcertHallReverb").Init();
AddActor(new Actor(this, "QUARRYReverb", new Vector3(128, 0, 0), new Vector3(0, 0, 0), 0.0f));
GetActorByName("QUARRYReverb").Components.Add(new Engine.Components.ReverbComponent(this, GetActorByName("QUARRYReverb"), new Vector3(0, 0, 0), FMOD.PRESET.QUARRY(), 32, 32));
GetActorByName("QUARRYReverb").Init();
AddActor(new Actor(this, "UNDERWATEReverb", new Vector3(0, 128, 0), new Vector3(0, 0, 0), 0.0f));
GetActorByName("UNDERWATEReverb").Components.Add(new Engine.Components.ReverbComponent(this, GetActorByName("UNDERWATEReverb"), new Vector3(0, 0, 0), FMOD.PRESET.UNDERWATER(), 32, 32));
GetActorByName("UNDERWATEReverb").Init();
AddActor(new Player(this, "player", new Vector3(0, 0, 0), new Vector3(0, 0, 0), 0.0f));
// GetActorByName("player").Components.Add(new Engine.Components.BasicMovementComponent(this, GetActorByName("player")));
GetActorByName("player").Components.Add(new Engine.Components.ImageDisplayComponent(this, GetActorByName("player"), "Textures/solid"));
GetActorByName("player").Init();
currentCamera = (GetActorByName("player") as Player).playerCamera;
}
private void GetRandomAABB(ref AABB aabb)
{
Vec2 w = new Vec2(); w.Set(k_width, k_width);
aabb.LowerBound.X = Box2DXMath.Random(-k_extent, k_extent);
aabb.LowerBound.Y = Box2DXMath.Random(0.0f, 2.0f * k_extent);
aabb.UpperBound = aabb.LowerBound + w;
}
private void MoveAABB(ref AABB aabb)
{
Vec2 d = new Vec2();
d.X = Box2DXMath.Random(-0.5f, 0.5f);
d.Y = Box2DXMath.Random(-0.5f, 0.5f);
//d.x = 2.0f;
//d.y = 0.0f;
aabb.LowerBound += d;
aabb.UpperBound += d;
Vec2 c0 = 0.5f * (aabb.LowerBound + aabb.UpperBound);
Vec2 min = new Vec2(); min.Set(-k_extent, 0.0f);
Vec2 max = new Vec2(); max.Set(k_extent, 2.0f * k_extent);
Vec2 c = Box2DXMath.Clamp(c0, min, max);
aabb.LowerBound += c - c0;
aabb.UpperBound += c - c0;
}
// Create and destroy proxies. These call Flush first.
public ushort CreateProxy(AABB aabb, object userData)
{
Box2DXDebug.Assert(_proxyCount < Settings.MaxProxies);
Box2DXDebug.Assert(_freeProxy != PairManager.NullProxy);
ushort proxyId = _freeProxy;
Proxy proxy = _proxyPool[proxyId];
_freeProxy = proxy.Next;
proxy.OverlapCount = 0;
proxy.UserData = userData;
int boundCount = 2 * _proxyCount;
ushort[] lowerValues = new ushort[2], upperValues = new ushort[2];
ComputeBounds(out lowerValues, out upperValues, aabb);
for (int axis = 0; axis < 2; ++axis)
{
Bound[] bounds = _bounds[axis];
int lowerIndex, upperIndex;
Query(out lowerIndex, out upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
#warning "Check this"
//memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound));
Bound[] tmp = new Bound[boundCount - upperIndex];
for (int i = 0; i < (boundCount - upperIndex); i++)
{
tmp[i] = bounds[upperIndex + i].Clone();
}
for (int i = 0; i < (boundCount - upperIndex); i++)
{
bounds[upperIndex + 2 + i] = tmp[i];
}
//memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound));
tmp = new Bound[upperIndex - lowerIndex];
for (int i = 0; i < (upperIndex - lowerIndex); i++)
{
tmp[i] = bounds[lowerIndex + i].Clone();
}
for (int i = 0; i < (upperIndex - lowerIndex); i++)
{
bounds[lowerIndex + 1 + i] = tmp[i];
}
// The upper index has increased because of the lower bound insertion.
++upperIndex;
// Copy in the new bounds.
bounds[lowerIndex].Value = lowerValues[axis];
bounds[lowerIndex].ProxyId = proxyId;
bounds[upperIndex].Value = upperValues[axis];
bounds[upperIndex].ProxyId = proxyId;
bounds[lowerIndex].StabbingCount = lowerIndex == 0 ? (ushort)0 : bounds[lowerIndex - 1].StabbingCount;
bounds[upperIndex].StabbingCount = bounds[upperIndex - 1].StabbingCount;
// Adjust the stabbing count between the new bounds.
for (int index = lowerIndex; index < upperIndex; ++index)
{
++bounds[index].StabbingCount;
}
// Adjust the all the affected bound indices.
for (int index = lowerIndex; index < boundCount + 2; ++index)
{
Proxy proxy_ = _proxyPool[bounds[index].ProxyId];
if (bounds[index].IsLower)
{
proxy_.LowerBounds[axis] = (ushort)index;
}
else
{
proxy_.UpperBounds[axis] = (ushort)index;
}
}
}
++_proxyCount;
Box2DXDebug.Assert(_queryResultCount < Settings.MaxProxies);
// Create pairs if the AABB is in range.
for (int i = 0; i < _queryResultCount; ++i)
{
Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
Box2DXDebug.Assert(_proxyPool[_queryResults[i]].IsValid);
_pairManager.AddBufferedPair(proxyId, _queryResults[i]);
}
_pairManager.Commit();
if (IsValidate)
{
Validate();
}
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
return(proxyId);
}
public abstract void ComputeAABB(out AABB aabb, XForm xf);
/// <summary>
/// Combine two AABBs into this one.
/// </summary>
public void Combine(AABB aabb1, AABB aabb2)
{
LowerBound = Math.Min(aabb1.LowerBound, aabb2.LowerBound);
UpperBound = Math.Max(aabb1.UpperBound, aabb2.UpperBound);
}
public ushort CreateProxy(AABB aabb, object userData)
{
Box2DXDebug.Assert(this._proxyCount < Settings.MaxProxies);
Box2DXDebug.Assert(this._freeProxy != PairManager.NullProxy);
ushort freeProxy = this._freeProxy;
Proxy proxy = this._proxyPool[(int)freeProxy];
this._freeProxy = proxy.Next;
proxy.OverlapCount = 0;
proxy.UserData = userData;
int num = 2 * this._proxyCount;
ushort[] array = new ushort[2];
ushort[] array2 = new ushort[2];
this.ComputeBounds(out array, out array2, aabb);
for (int i = 0; i < 2; i++)
{
Bound[] array3 = this._bounds[i];
int num2;
int num3;
this.Query(out num2, out num3, array[i], array2[i], array3, num, i);
Bound[] array4 = new Bound[num - num3];
for (int j = 0; j < num - num3; j++)
{
array4[j] = array3[num3 + j].Clone();
}
for (int j = 0; j < num - num3; j++)
{
array3[num3 + 2 + j] = array4[j];
}
array4 = new Bound[num3 - num2];
for (int j = 0; j < num3 - num2; j++)
{
array4[j] = array3[num2 + j].Clone();
}
for (int j = 0; j < num3 - num2; j++)
{
array3[num2 + 1 + j] = array4[j];
}
num3++;
array3[num2].Value = array[i];
array3[num2].ProxyId = freeProxy;
array3[num3].Value = array2[i];
array3[num3].ProxyId = freeProxy;
array3[num2].StabbingCount = ((num2 == 0) ? (ushort)0 : array3[num2 - 1].StabbingCount);
array3[num3].StabbingCount = array3[num3 - 1].StabbingCount;
for (int k = num2; k < num3; k++)
{
Bound expr_1E4 = array3[k];
expr_1E4.StabbingCount += 1;
}
for (int k = num2; k < num + 2; k++)
{
Proxy proxy2 = this._proxyPool[(int)array3[k].ProxyId];
if (array3[k].IsLower)
{
proxy2.LowerBounds[i] = (ushort)k;
}
else
{
proxy2.UpperBounds[i] = (ushort)k;
}
}
}
this._proxyCount++;
Box2DXDebug.Assert(this._queryResultCount < Settings.MaxProxies);
for (int j = 0; j < this._queryResultCount; j++)
{
Box2DXDebug.Assert((int)this._queryResults[j] < Settings.MaxProxies);
Box2DXDebug.Assert(this._proxyPool[(int)this._queryResults[j]].IsValid);
this._pairManager.AddBufferedPair((int)freeProxy, (int)this._queryResults[j]);
}
this._pairManager.Commit();
if (BroadPhase.IsValidate)
{
this.Validate();
}
this._queryResultCount = 0;
this.IncrementTimeStamp();
return freeProxy;
}
public override void ComputeAABB(out AABB aabb, XForm xf)
{
Mat22 R = Common.Math.Mul(xf.R, _obb.R);
Mat22 absR = Common.Math.Abs(R);
Vec2 h = Common.Math.Mul(absR, _obb.Extents);
Vec2 position = xf.Position + Common.Math.Mul(xf.R, _obb.Center);
aabb.LowerBound = position - h;
aabb.UpperBound = position + h;
}
public void MoveProxy(int proxyId, AABB aabb)
{
if (proxyId == (int)PairManager.NullProxy || Settings.MaxProxies <= proxyId)
{
Box2DXDebug.Assert(false);
}
else
{
if (!aabb.IsValid)
{
Box2DXDebug.Assert(false);
}
else
{
int num = 2 * this._proxyCount;
Proxy proxy = this._proxyPool[proxyId];
BoundValues boundValues = new BoundValues();
this.ComputeBounds(out boundValues.LowerValues, out boundValues.UpperValues, aabb);
BoundValues boundValues2 = new BoundValues();
for (int i = 0; i < 2; i++)
{
boundValues2.LowerValues[i] = this._bounds[i][(int)proxy.LowerBounds[i]].Value;
boundValues2.UpperValues[i] = this._bounds[i][(int)proxy.UpperBounds[i]].Value;
}
for (int i = 0; i < 2; i++)
{
Bound[] array = this._bounds[i];
int num2 = (int)proxy.LowerBounds[i];
int num3 = (int)proxy.UpperBounds[i];
ushort num4 = boundValues.LowerValues[i];
ushort num5 = boundValues.UpperValues[i];
int num6 = (int)(num4 - array[num2].Value);
int num7 = (int)(num5 - array[num3].Value);
array[num2].Value = num4;
array[num3].Value = num5;
if (num6 < 0)
{
int num8 = num2;
while (num8 > 0 && num4 < array[num8 - 1].Value)
{
Bound bound = array[num8];
Bound bound2 = array[num8 - 1];
int proxyId2 = (int)bound2.ProxyId;
Proxy proxy2 = this._proxyPool[(int)bound2.ProxyId];
Bound expr_18A = bound2;
expr_18A.StabbingCount += 1;
if (bound2.IsUpper)
{
if (this.TestOverlap(boundValues, proxy2))
{
this._pairManager.AddBufferedPair(proxyId, proxyId2);
}
ushort[] expr_1DA_cp_0 = proxy2.UpperBounds;
int expr_1DA_cp_1 = i;
expr_1DA_cp_0[expr_1DA_cp_1] += 1;
Bound expr_1EA = bound;
expr_1EA.StabbingCount += 1;
}
else
{
ushort[] expr_20A_cp_0 = proxy2.LowerBounds;
int expr_20A_cp_1 = i;
expr_20A_cp_0[expr_20A_cp_1] += 1;
Bound expr_21A = bound;
expr_21A.StabbingCount -= 1;
}
ushort[] expr_236_cp_0 = proxy.LowerBounds;
int expr_236_cp_1 = i;
expr_236_cp_0[expr_236_cp_1] -= 1;
Box2DX.Common.Math.Swap<Bound>(ref array[num8], ref array[num8 - 1]);
num8--;
}
}
if (num7 > 0)
{
int num8 = num3;
while (num8 < num - 1 && array[num8 + 1].Value <= num5)
{
Bound bound = array[num8];
Bound bound3 = array[num8 + 1];
int proxyId3 = (int)bound3.ProxyId;
Proxy proxy3 = this._proxyPool[proxyId3];
Bound expr_2C9 = bound3;
expr_2C9.StabbingCount += 1;
if (bound3.IsLower)
{
if (this.TestOverlap(boundValues, proxy3))
{
this._pairManager.AddBufferedPair(proxyId, proxyId3);
}
ushort[] expr_319_cp_0 = proxy3.LowerBounds;
int expr_319_cp_1 = i;
expr_319_cp_0[expr_319_cp_1] -= 1;
Bound expr_329 = bound;
expr_329.StabbingCount += 1;
}
else
{
ushort[] expr_349_cp_0 = proxy3.UpperBounds;
int expr_349_cp_1 = i;
expr_349_cp_0[expr_349_cp_1] -= 1;
Bound expr_359 = bound;
expr_359.StabbingCount -= 1;
}
ushort[] expr_375_cp_0 = proxy.UpperBounds;
int expr_375_cp_1 = i;
expr_375_cp_0[expr_375_cp_1] += 1;
Box2DX.Common.Math.Swap<Bound>(ref array[num8], ref array[num8 + 1]);
num8++;
}
}
if (num6 > 0)
{
int num8 = num2;
while (num8 < num - 1 && array[num8 + 1].Value <= num4)
{
Bound bound = array[num8];
Bound bound3 = array[num8 + 1];
int proxyId3 = (int)bound3.ProxyId;
Proxy proxy3 = this._proxyPool[proxyId3];
Bound expr_40D = bound3;
expr_40D.StabbingCount -= 1;
if (bound3.IsUpper)
{
if (this.TestOverlap(boundValues2, proxy3))
{
this._pairManager.RemoveBufferedPair(proxyId, proxyId3);
}
ushort[] expr_45D_cp_0 = proxy3.UpperBounds;
int expr_45D_cp_1 = i;
expr_45D_cp_0[expr_45D_cp_1] -= 1;
Bound expr_46D = bound;
expr_46D.StabbingCount -= 1;
}
else
{
ushort[] expr_48D_cp_0 = proxy3.LowerBounds;
int expr_48D_cp_1 = i;
expr_48D_cp_0[expr_48D_cp_1] -= 1;
Bound expr_49D = bound;
expr_49D.StabbingCount += 1;
}
ushort[] expr_4B9_cp_0 = proxy.LowerBounds;
int expr_4B9_cp_1 = i;
expr_4B9_cp_0[expr_4B9_cp_1] += 1;
Box2DX.Common.Math.Swap<Bound>(ref array[num8], ref array[num8 + 1]);
num8++;
}
}
if (num7 < 0)
{
int num8 = num3;
while (num8 > 0 && num5 < array[num8 - 1].Value)
{
Bound bound = array[num8];
Bound bound2 = array[num8 - 1];
int proxyId2 = (int)bound2.ProxyId;
Proxy proxy2 = this._proxyPool[proxyId2];
Bound expr_551 = bound2;
expr_551.StabbingCount -= 1;
if (bound2.IsLower)
{
if (this.TestOverlap(boundValues2, proxy2))
{
this._pairManager.RemoveBufferedPair(proxyId, proxyId2);
}
ushort[] expr_5A1_cp_0 = proxy2.LowerBounds;
int expr_5A1_cp_1 = i;
expr_5A1_cp_0[expr_5A1_cp_1] += 1;
Bound expr_5B1 = bound;
expr_5B1.StabbingCount -= 1;
}
else
{
ushort[] expr_5D1_cp_0 = proxy2.UpperBounds;
int expr_5D1_cp_1 = i;
expr_5D1_cp_0[expr_5D1_cp_1] += 1;
Bound expr_5E1 = bound;
expr_5E1.StabbingCount += 1;
}
ushort[] expr_5FD_cp_0 = proxy.UpperBounds;
int expr_5FD_cp_1 = i;
expr_5FD_cp_0[expr_5FD_cp_1] -= 1;
Box2DX.Common.Math.Swap<Bound>(ref array[num8], ref array[num8 - 1]);
num8--;
}
}
}
if (BroadPhase.IsValidate)
{
this.Validate();
}
}
}
}
// Use this to see if your proxy is in range. If it is not in range,
// it should be destroyed. Otherwise you may get O(m^2) pairs, where m
// is the number of proxies that are out of range.
public bool InRange(AABB aabb)
{
Vec2 d = Common.Math.Max(aabb.LowerBound - _worldAABB.UpperBound, _worldAABB.LowerBound - aabb.UpperBound);
return(Common.Math.Max(d.X, d.Y) < 0.0f);
}
public int Query(AABB aabb, object[] userData, int maxCount)
{
ushort[] array;
ushort[] array2;
this.ComputeBounds(out array, out array2, aabb);
int num;
int num2;
this.Query(out num, out num2, array[0], array2[0], this._bounds[0], 2 * this._proxyCount, 0);
this.Query(out num, out num2, array[1], array2[1], this._bounds[1], 2 * this._proxyCount, 1);
Box2DXDebug.Assert(this._queryResultCount < Settings.MaxProxies);
int num3 = 0;
int num4 = 0;
while (num4 < this._queryResultCount && num3 < maxCount)
{
Box2DXDebug.Assert((int)this._queryResults[num4] < Settings.MaxProxies);
Proxy proxy = this._proxyPool[(int)this._queryResults[num4]];
Box2DXDebug.Assert(proxy.IsValid);
userData[num4] = proxy.UserData;
num4++;
num3++;
}
this._queryResultCount = 0;
this.IncrementTimeStamp();
return num3;
}
public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2)
{
aabb = new AABB();
Vec2 p1 = transform1.Position + Common.Math.Mul(transform1.R, _localPosition);
Vec2 p2 = transform2.Position + Common.Math.Mul(transform2.R, _localPosition);
Vec2 lower = Common.Math.Min(p1, p2);
Vec2 upper = Common.Math.Max(p1, p2);
aabb.LowerBound.Set(lower.X - _radius, lower.Y - _radius);
aabb.UpperBound.Set(upper.X + _radius, upper.Y + _radius);
}
/// <summary>
/// Does this aabb contain the provided AABB
/// </summary>
public bool Contains(AABB aabb)
{
bool result = true;
result = result && LowerBound.X <= aabb.LowerBound.X;
result = result && LowerBound.Y <= aabb.LowerBound.Y;
result = result && aabb.UpperBound.X <= UpperBound.X;
result = result && aabb.UpperBound.Y <= UpperBound.Y;
return result;
}
/// Combine two AABBs into this one.
public void Combine(AABB aabb1, AABB aabb2)
{
LowerBound = Vector2.Min(aabb1.LowerBound, aabb2.LowerBound);
UpperBound = Vector2.Max(aabb1.UpperBound, aabb2.UpperBound);
}
/// <summary>
/// Mouse interaction event.
/// </summary>
public void MouseDown(Vec2 p)
{
if (_mouseJoint != null)
{
return;
}
// Make a small box.
AABB aabb = new AABB();
Vec2 d = new Vec2();
d.Set(0.001f, 0.001f);
aabb.LowerBound = p - d;
aabb.UpperBound = p + d;
// Query the world for overlapping shapes.
int k_maxCount = 10;
Shape[] shapes = new Shape[k_maxCount];
int count = _world.Query(aabb, shapes, k_maxCount);
Body body = null;
for (int i = 0; i < count; ++i)
{
Body shapeBody = shapes[i].GetBody();
if (shapeBody.IsStatic() == false && shapeBody.GetMass() > 0.0f)
{
bool inside = shapes[i].TestPoint(shapeBody.GetXForm(), p);
if (inside)
{
body = shapes[i].GetBody();
break;
}
}
}
if (body != null)
{
MouseJointDef md = new MouseJointDef();
md.Body1 = _world.GetGroundBody();
md.Body2 = body;
md.Target = p;
md.MaxForce = 1000.0f * body.GetMass();
md.FrequencyHz = 30f;
_mouseJoint = (MouseJoint)_world.CreateJoint(md);
body.WakeUp();
}
}
/// <summary>
/// Called to setup the arena boundaries and bind to all of the physics stuff.
/// </summary>
public virtual void Bind()
{
float width = dimensions.X, height = dimensions.Y;
worldAABB = new AABB();
worldAABB.LowerBound.Set(-width / 2 - edgeTolerance, -edgeTolerance);
worldAABB.UpperBound.Set(width / 2 + edgeTolerance, height + edgeTolerance);
Vec2 gravity = new Vec2(this.gravity.X, this.gravity.Y);
bool doSleep = true;
world = new World(worldAABB, gravity, doSleep);
world.SetContactFilter(new ContactFilter());
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(0.0f, 0.0f);
Body groundBody = world.CreateBody(groundBodyDef);
// Bottom
AddBoundaryBlock(groundBody, 0, -(boundaryThickness / 2), width + boundaryThickness * 2, boundaryThickness);
// Top
AddBoundaryBlock(groundBody, 0, height + boundaryThickness / 2, width + boundaryThickness * 2, boundaryThickness);
// Left
AddBoundaryBlock(groundBody, -(width / 2) - boundaryThickness / 2, height / 2, boundaryThickness, height + boundaryThickness * 2);
// Right
AddBoundaryBlock(groundBody, +(width / 2) + boundaryThickness / 2, height / 2, boundaryThickness, height + boundaryThickness * 2);
DebugDraw draw = new OpenTKDebugDraw();
draw.Flags = DebugDraw.DrawFlags.Shape;
if (Program.DebugDraw)
{
world.SetDebugDraw(draw);
}
world.SetContactListener(this);
// Old code for reference
/*
*
* _worldAABB = new AABB();
_worldAABB.LowerBound.Set(-30.0f, -20.0f);
_worldAABB.UpperBound.Set(30.0f, 40.0f);
Vec2 gravity = new Vec2();
gravity.Set(0.0f, -10.0f);
bool doSleep = true;
_world = new World(_worldAABB, gravity, doSleep);
_world.SetContactFilter(new ContactFilter());
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(0.0f, 0.0f);
Body groundBody = _world.CreateBody(groundBodyDef);
AddBlock(groundBody, 0, -5, 40, 10);
AddBlock(groundBody, -20, 20, 1, 40);
AddBlock(groundBody, 20, 20, 1, 40);
AddBlock(groundBody, 0, 30, 40, 10);
DebugDraw draw = new OpenTKDebugDraw();
draw.Flags = DebugDraw.DrawFlags.Shape;
if (Program.DebugDraw) {
_world.SetDebugDraw(draw);
}
_world.SetContactListener(this);
*/
}
public override void ComputeSweptAABB(out AABB aabb, XForm transform1, XForm transform2)
{
AABB aabb1, aabb2;
ComputeAABB(out aabb1, transform1);
ComputeAABB(out aabb2, transform2);
aabb.LowerBound = Common.Math.Min(aabb1.LowerBound, aabb2.LowerBound);
aabb.UpperBound = Common.Math.Max(aabb1.UpperBound, aabb2.UpperBound);
}
private void ComputeBounds(out ushort[] lowerValues, out ushort[] upperValues, AABB aabb)
{
lowerValues = new ushort[2];
upperValues = new ushort[2];
Box2DXDebug.Assert(aabb.UpperBound.X >= aabb.LowerBound.X);
Box2DXDebug.Assert(aabb.UpperBound.Y >= aabb.LowerBound.Y);
Vec2 minVertex = Common.Math.Clamp(aabb.LowerBound, _worldAABB.LowerBound, _worldAABB.UpperBound);
Vec2 maxVertex = Common.Math.Clamp(aabb.UpperBound, _worldAABB.LowerBound, _worldAABB.UpperBound);
// Bump lower bounds downs and upper bounds up. This ensures correct sorting of
// lower/upper bounds that would have equal values.
// TODO_ERIN implement fast float to uint16 conversion.
lowerValues[0] = (ushort)((ushort)(_quantizationFactor.X * (minVertex.X - _worldAABB.LowerBound.X)) & (BROADPHASE_MAX - 1));
upperValues[0] = (ushort)((ushort)(_quantizationFactor.X * (maxVertex.X - _worldAABB.LowerBound.X)) | 1);
lowerValues[1] = (ushort)((ushort)(_quantizationFactor.Y * (minVertex.Y - _worldAABB.LowerBound.Y)) & (BROADPHASE_MAX - 1));
upperValues[1] = (ushort)((ushort)(_quantizationFactor.Y * (maxVertex.Y - _worldAABB.LowerBound.Y)) | 1);
}