public virtual void SynchronizeFixtures()
{
/*
* b2Transform xf1 = b2Transform.Identity;
* xf1.q.Set(Sweep.a0);
* xf1.p = Sweep.c0 - b2Math.b2Mul(xf1.q, Sweep.localCenter);
*
* b2BroadPhase broadPhase = World.ContactManager.BroadPhase;
* for (b2Fixture f = FixtureList; f != null; f = f.Next)
* {
* f.Synchronize(broadPhase, ref xf1, ref Transform);
* }
*/
b2Transform xf1;
xf1.q.s = (float)Math.Sin(Sweep.a0);
xf1.q.c = (float)Math.Cos(Sweep.a0);
xf1.p.x = Sweep.c0.x - (xf1.q.c * Sweep.localCenter.x - xf1.q.s * Sweep.localCenter.y);
xf1.p.y = Sweep.c0.y - (xf1.q.s * Sweep.localCenter.x + xf1.q.c * Sweep.localCenter.y);
b2BroadPhase broadPhase = World.ContactManager.BroadPhase;
for (b2Fixture f = FixtureList; f != null; f = f.Next)
{
f.Synchronize(broadPhase, ref xf1, ref Transform);
}
}
public void DrawFixture(b2Fixture fixture)
{
b2Color color = new b2Color(0.95f, 0.95f, 0.6f);
b2Transform xf = fixture.Body.Transform;
switch (fixture.ShapeType)
{
case b2ShapeType.e_circle:
{
b2CircleShape circle = (b2CircleShape) fixture.Shape;
b2Vec2 center = b2Math.b2Mul(xf, circle.Position);
float radius = circle.Radius;
m_debugDraw.DrawCircle(center, radius, color);
}
break;
case b2ShapeType.e_polygon:
{
b2PolygonShape poly = (b2PolygonShape) fixture.Shape;
int vertexCount = poly.VertexCount;
Debug.Assert(vertexCount <= b2Settings.b2_maxPolygonVertices);
b2Vec2[] vertices = new b2Vec2[b2Settings.b2_maxPolygonVertices];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = b2Math.b2Mul(xf, poly.Vertices[i]);
}
m_debugDraw.DrawPolygon(vertices, vertexCount, color);
}
break;
}
}
public virtual void Dump()
{
int bodyIndex = m_islandIndex;
System.Diagnostics.Debug.WriteLine("{");
System.Diagnostics.Debug.WriteLine(" b2BodyDef bd;");
System.Diagnostics.Debug.WriteLine(" bd.type = {0};", m_type);
System.Diagnostics.Debug.WriteLine(" bd.position.Set({0:N5}, {1:N5});", m_xf.p.x, m_xf.p.y);
System.Diagnostics.Debug.WriteLine(" bd.angle = {0:N5};", Sweep.a);
System.Diagnostics.Debug.WriteLine(" bd.linearVelocity.Set({0:N5}, {1:N5});", m_linearVelocity.x, m_linearVelocity.y);
System.Diagnostics.Debug.WriteLine(" bd.angularVelocity = {0:N5};", m_angularVelocity);
System.Diagnostics.Debug.WriteLine(" bd.linearDamping = {0:N5};", m_linearDamping);
System.Diagnostics.Debug.WriteLine(" bd.angularDamping = {0:N5};", m_angularDamping);
System.Diagnostics.Debug.WriteLine(" bd.allowSleep = {0};", m_flags.HasFlag(b2BodyFlags.e_autoSleepFlag));
System.Diagnostics.Debug.WriteLine(" bd.awake = {0};", m_flags.HasFlag(b2BodyFlags.e_awakeFlag));
System.Diagnostics.Debug.WriteLine(" bd.fixedRotation = {0};", m_flags.HasFlag(b2BodyFlags.e_fixedRotationFlag));
System.Diagnostics.Debug.WriteLine(" bd.bullet = {0};", m_flags.HasFlag(b2BodyFlags.e_bulletFlag));
System.Diagnostics.Debug.WriteLine(" bd.active = {0};", m_flags.HasFlag(b2BodyFlags.e_activeFlag));
System.Diagnostics.Debug.WriteLine(" bd.gravityScale = {0:N5};", m_gravityScale);
System.Diagnostics.Debug.WriteLine(" bodies[{0}] = m_world.CreateBody(bd);", m_islandIndex);
System.Diagnostics.Debug.WriteLine("");
for (b2Fixture f = m_fixtureList; f != null; f = f.Next)
{
System.Diagnostics.Debug.WriteLine(" {");
f.Dump(bodyIndex);
System.Diagnostics.Debug.WriteLine(" }");
}
System.Diagnostics.Debug.WriteLine("}");
}
public virtual b2Fixture CreateFixture(b2FixtureDef def)
{
if (m_world.IsLocked == true)
{
return(null);
}
b2Fixture fixture = new b2Fixture();
fixture.Create(this, def);
if (m_flags.HasFlag(b2BodyFlags.e_activeFlag))
{
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
fixture.CreateProxies(broadPhase, m_xf);
}
fixture.Next = m_fixtureList;
m_fixtureList = fixture;
++m_fixtureCount;
fixture.Body = this;
// Adjust mass properties if needed.
if (fixture.Density > 0.0f)
{
ResetMassData();
}
// Let the world know we have a new fixture. This will cause new contacts
// to be created at the beginning of the next time step.
m_world.Flags |= b2WorldFlags.e_newFixture;
return(fixture);
}
public void Create(b2Body body, b2FixtureDef def)
{
UserData = def.userData;
Friction = def.friction;
Restitution = def.restitution;
Body = body;
Next = null;
m_filter = def.filter;
m_isSensor = def.isSensor;
Shape = def.shape.Clone();
// Reserve proxy space
int childCount = Shape.GetChildCount();
m_proxies = b2ArrayPool <b2FixtureProxy> .Create(childCount, true);
for (int i = 0; i < childCount; ++i)
{
m_proxies[i].fixture = null;
m_proxies[i].proxyId = b2BroadPhase.e_nullProxy;
}
m_proxyCount = 0;
Density = def.density;
}
public virtual void SetTransform(b2Vec2 position, float angle)
{
if (m_world.IsLocked == true)
{
return;
}
m_xf.q.Set(angle);
m_xf.p = position;
Sweep.c = b2Math.b2Mul(m_xf, Sweep.localCenter);
Sweep.a = angle;
Sweep.c0 = Sweep.c;
Sweep.a0 = angle;
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
for (b2Fixture f = m_fixtureList; f != null; f = f.Next)
{
f.Synchronize(broadPhase, m_xf, m_xf);
}
m_world.ContactManager.FindNewContacts();
}
public void Create(b2Body body, b2FixtureDef def)
{
m_userData = def.userData;
m_friction = def.friction;
m_restitution = def.restitution;
m_body = body;
m_next = null;
m_filter = def.filter;
m_isSensor = def.isSensor;
m_shape = def.shape.Clone();
// Reserve proxy space
int childCount = m_shape.GetChildCount();
for (int i = 0; i < childCount; ++i)
{
b2FixtureProxy proxy = new b2FixtureProxy();
proxy.fixture = null;
proxy.proxyId = b2BroadPhase.e_nullProxy;
m_proxies.Add(proxy);
}
m_proxyCount = 0;
m_density = def.density;
}
/**
* Set if this fixture is a sensor.
*/
public void SetSensor(bool sensor)
{
if (m_isSensor == sensor)
{
return;
}
m_isSensor = sensor;
if (m_body == null)
{
return;
}
b2ContactEdge edge = m_body.GetContactList();
while (edge != null)
{
b2Contact contact = edge.contact;
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA == this || fixtureB == this)
{
contact.SetSensor(fixtureA.IsSensor() || fixtureB.IsSensor());
}
edge = edge.next;
}
}
/**
* Return true if the given fixture should be considered for ray intersection.
* By default, userData is cast as a b2Fixture and collision is resolved according to ShouldCollide
* @see ShouldCollide()
* @see b2World#Raycast
* @param userData arbitrary data passed from Raycast or RaycastOne
* @param fixture the fixture that we are testing for filtering
* @return a Boolean, with a value of false indicating that this fixture should be ignored.
*/
public bool RayCollide(object userData, b2Fixture fixture)
{
if (userData == null)
{
return(true);
}
return(ShouldCollide(userData as b2Fixture, fixture));
}
/// Return true if contact calculations should be performed between these two shapes.
/// @warning for performance reasons this is only called when the AABBs begin to overlap.
public virtual bool ShouldCollide(b2Fixture fixtureA, b2Fixture fixtureB)
{
b2Filter filterA = fixtureA.Filter;
b2Filter filterB = fixtureB.Filter;
if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0)
{
return filterA.groupIndex > 0;
}
bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;
return collide;
}
/**
* Return true if contact calculations should be performed between these two fixtures.
* @warning for performance reasons this is only called when the AABBs begin to overlap.
*/
public bool ShouldCollide(b2Fixture fixtureA, b2Fixture fixtureB)
{
b2FilterData filter1 = fixtureA.GetFilterData();
b2FilterData filter2 = fixtureB.GetFilterData();
if (filter1.groupIndex == filter2.groupIndex && filter1.groupIndex != 0)
{
return(filter1.groupIndex > 0);
}
bool collide = (filter1.maskBits & filter2.categoryBits) != 0 && (filter1.categoryBits & filter2.maskBits) != 0;
return(collide);
}
public virtual void SynchronizeFixtures()
{
b2Transform xf1 = b2Transform.Create();
xf1.q.Set(Sweep.a0);
xf1.p = Sweep.c0 - b2Math.b2Mul(xf1.q, Sweep.localCenter);
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
for (b2Fixture f = m_fixtureList; f != null; f = f.Next)
{
f.Synchronize(broadPhase, xf1, m_xf);
}
}
/// Return true if contact calculations should be performed between these two shapes.
/// @warning for performance reasons this is only called when the AABBs begin to overlap.
public virtual bool ShouldCollide(b2Fixture fixtureA, b2Fixture fixtureB)
{
b2Filter filterA = fixtureA.Filter;
b2Filter filterB = fixtureB.Filter;
if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0)
{
return(filterA.groupIndex > 0);
}
bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;
return(collide);
}
public float RayCastCallback(ref b2RayCastInput input, int proxyId)
{
b2FixtureProxy proxy = broadPhase.GetUserData(proxyId);
b2Fixture fixture = proxy.fixture;
int index = proxy.childIndex;
b2RayCastOutput output;
bool hit = fixture.RayCast(out output, input, index);
if (hit)
{
float fraction = output.fraction;
b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
return(callback.ReportFixture(fixture, point, output.normal, fraction));
}
return(input.maxFraction);
}
public override bool ReportFixture(b2Fixture fixture)
{
b2Body body = fixture.Body;
if (body.BodyType == b2BodyType.b2_dynamicBody)
{
bool inside = fixture.TestPoint(m_point);
if (inside)
{
m_fixture = fixture;
// We are done, terminate the query.
return false;
}
}
// Continue the query.
return true;
}
public virtual void SetActive(bool flag)
{
if (flag == IsActive())
{
return;
}
if (flag)
{
m_flags |= b2BodyFlags.e_activeFlag;
// Create all proxies.
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
for (b2Fixture f = m_fixtureList; f != null; f = f.Next)
{
f.CreateProxies(broadPhase, m_xf);
}
// Contacts are created the next time step.
}
else
{
m_flags &= ~b2BodyFlags.e_activeFlag;
// Destroy all proxies.
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
for (b2Fixture f = m_fixtureList; f != null; f = f.Next)
{
f.DestroyProxies(broadPhase);
}
// Destroy the attached contacts.
b2ContactEdge ce = m_contactList;
while (ce != null)
{
b2ContactEdge ce0 = ce;
ce = ce.Next;
m_world.ContactManager.Destroy(ce0.Contact);
}
m_contactList = null;
}
}
public override float ReportFixture(b2Fixture fixture, b2Vec2 point,
b2Vec2 normal, float fraction)
{
b2Body body = fixture.Body;
object userData = body.UserData;
if (userData != null)
{
int index = (int) userData;
if (index == 0)
{
// filter
return -1.0f;
}
}
m_hit = true;
m_point = point;
m_normal = normal;
return 0.0f;
}
/// Called for each fixture found in the query AABB.
/// @return false to terminate the query.
public override bool ReportFixture(b2Fixture fixture)
{
if (m_count == e_maxCount)
{
return false;
}
b2Body body = fixture.Body;
b2Shape shape = fixture.Shape;
var transform = body.Transform;
bool overlap = b2Collision.b2TestOverlap(shape, 0, m_circle, 0, ref transform, ref m_transform);
if (overlap)
{
DrawFixture(fixture);
++m_count;
}
return true;
}
public virtual void Refilter()
{
if (m_body == null)
{
return;
}
// Flag associated contacts for filtering.
b2ContactEdge edge = m_body.ContactList;
while (edge != null)
{
b2Contact contact = edge.Contact;
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA == this || fixtureB == this)
{
contact.FlagForFiltering();
}
edge = edge.Next;
}
b2World world = m_body.World;
if (world == null)
{
return;
}
// Touch each proxy so that new pairs may be created
b2BroadPhase broadPhase = world.ContactManager.BroadPhase;
for (int i = 0; i < m_proxyCount; ++i)
{
broadPhase.TouchProxy(m_proxies[i].proxyId);
}
}
/**
* Set the contact filtering data. This will not update contacts until the next time
* step when either parent body is active and awake.
*/
public void SetFilterData(b2FilterData filter)
{
m_filter = filter.Copy();
if (m_body != null)
{
return;
}
b2ContactEdge edge = m_body.GetContactList();
while (edge != null)
{
b2Contact contact = edge.contact;
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
if (fixtureA == this || fixtureB == this)
{
contact.FlagForFiltering();
}
edge = edge.next;
}
}
public virtual void SetType(b2BodyType type)
{
if (m_world.IsLocked == true)
{
return;
}
if (m_type == type)
{
return;
}
m_type = type;
ResetMassData();
if (m_type == b2BodyType.b2_staticBody)
{
m_linearVelocity.SetZero();
m_angularVelocity = 0.0f;
Sweep.a0 = Sweep.a;
Sweep.c0 = Sweep.c;
SynchronizeFixtures();
}
SetAwake(true);
m_force.SetZero();
m_torque = 0.0f;
// Since the body type changed, we need to flag contacts for filtering.
for (b2Fixture f = m_fixtureList; f != null; f = f.Next)
{
f.Refilter();
}
}
public override float ReportFixture(b2Fixture fixture, b2Vec2 point,
b2Vec2 normal, float fraction)
{
b2Body body = fixture.Body;
object userData = body.UserData;
if (userData != null)
{
int index = (int) userData;
if (index == 0)
{
// filter
return -1.0f;
}
}
Debug.Assert(m_count < e_maxCount);
m_points[m_count] = point;
m_normals[m_count] = normal;
++m_count;
if (m_count == e_maxCount)
{
return 0.0f;
}
return 1.0f;
}
public JObject B2n(b2Fixture fixture)
{
JObject fixtureValue = new JObject();
String fixtureName = GetFixtureName(fixture);
if (null != fixtureName)
fixtureValue["name"] = fixtureName;
if (fixture.Restitution != 0)
FloatToJson("restitution", fixture.Restitution, fixtureValue);
if (fixture.Friction != 0)
FloatToJson("friction", fixture.Friction, fixtureValue);
if (fixture.Density != 0)
FloatToJson("density", fixture.Density, fixtureValue);
if (fixture.IsSensor)
fixtureValue["sensor"] = true;
if ((int)fixture.Filter.categoryBits != 0x0001)
fixtureValue["filter-categoryBits"] = (int)fixture.Filter.categoryBits;
if ((int)fixture.Filter.maskBits != 0xffff)
fixtureValue["filter-maskBits"] = (int)fixture.Filter.maskBits;
if (fixture.Filter.groupIndex != 0)
fixtureValue["filter-groupIndex"] = fixture.Filter.groupIndex;
b2Shape shape = fixture.Shape;
switch (shape.ShapeType)
{
case b2ShapeType.e_circle:
{
b2CircleShape circle = (b2CircleShape)shape;
JObject shapeValue = new JObject();
FloatToJson("radius", circle.Radius, shapeValue);
VecToJson("center", circle.Position, shapeValue);
fixtureValue["circle"] = shapeValue;
}
break;
case b2ShapeType.e_edge:
{
b2EdgeShape edge = (b2EdgeShape)shape;
JObject shapeValue = new JObject();
VecToJson("vertex1", edge.Vertex1, shapeValue);
VecToJson("vertex2", edge.Vertex2, shapeValue);
if (edge.HasVertex0)
shapeValue["hasVertex0"] = true;
if (edge.HasVertex3)
shapeValue["hasVertex3"] = true;
if (edge.HasVertex0)
VecToJson("vertex0", edge.Vertex0, shapeValue);
if (edge.HasVertex3)
VecToJson("vertex3", edge.Vertex3, shapeValue);
fixtureValue["edge"] = shapeValue;
}
break;
case b2ShapeType.e_chain:
{
b2ChainShape chain = (b2ChainShape)shape;
JObject shapeValue = new JObject();
int count = chain.Vertices.Length;
for (int i = 0; i < count; ++i)
VecToJson("vertices", chain.Vertices[i], shapeValue, i);
if (chain.PrevVertex != null && chain.PrevVertex != b2Vec2.Zero)
{
shapeValue["hasPrevVertex"] = true;
VecToJson("prevVertex", chain.PrevVertex, shapeValue);
}
if (chain.NextVertex != null && chain.NextVertex != b2Vec2.Zero)
{
shapeValue["hasNextVertex"] = true;
VecToJson("nextVertex", chain.NextVertex, shapeValue);
}
fixtureValue["chain"] = shapeValue;
}
break;
case b2ShapeType.e_polygon:
{
b2PolygonShape poly = (b2PolygonShape)shape;
JObject shapeValue = new JObject();
int vertexCount = poly.Vertices.Length;
for (int i = 0; i < vertexCount; ++i)
VecToJson("vertices", poly.Vertices[i], shapeValue, i);
fixtureValue["polygon"] = shapeValue;
}
break;
default:
Console.WriteLine("Unknown shape type : " + shape.ShapeType);
break;
}
JArray customPropertyValue = WriteCustomPropertiesToJson(fixture);
if (customPropertyValue.Count > 0)
fixtureValue["customProperties"] = customPropertyValue;
return fixtureValue;
}
public void DrawShape(b2Fixture fixture, b2Transform xf, b2Color color)
{
switch (fixture.ShapeType)
{
case b2ShapeType.e_circle:
{
b2CircleShape circle = (b2CircleShape)fixture.Shape;
b2Vec2 center = b2Math.b2Mul(xf, circle.Position);
float radius = circle.Radius;
b2Vec2 axis = b2Math.b2Mul(xf.q, new b2Vec2(1.0f, 0.0f));
m_debugDraw.DrawSolidCircle(center, radius, axis, color);
}
break;
case b2ShapeType.e_edge:
{
b2EdgeShape edge = (b2EdgeShape)fixture.Shape;
b2Vec2 v1 = b2Mul(xf, edge.m_vertex1);
b2Vec2 v2 = b2Mul(xf, edge.m_vertex2);
m_debugDraw.DrawSegment(v1, v2, color);
}
break;
case b2ShapeType.e_chain:
{
b2ChainShape chain = (b2ChainShape)fixture.Shape;
int count = chain.Count;
b2Vec2[] vertices = chain.Vertices;
b2Vec2 v1 = b2Math.b2Mul(xf, vertices[0]);
for (int i = 1; i < count; ++i)
{
b2Vec2 v2 = b2Math.b2Mul(xf, vertices[i]);
m_debugDraw.DrawSegment(v1, v2, color);
m_debugDraw.DrawCircle(v1, 0.05f, color);
v1 = v2;
}
}
break;
case b2ShapeType.e_polygon:
{
b2PolygonShape poly = (b2PolygonShape)fixture.Shape;
int vertexCount = poly.VertexCount;
b2Vec2[] vertices = new b2Vec2[b2Settings.b2_maxPolygonVertices];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = b2Math.b2Mul(xf, poly.Vertices[i]);
}
m_debugDraw.DrawSolidPolygon(vertices, vertexCount, color);
}
break;
default:
break;
}
}
/// <summary> /// Called for each fixture found in the query AABB. /// @return false to terminate the query. /// </summary> public abstract bool ReportFixture(b2Fixture fixture);
public virtual void DestroyFixture(b2Fixture fixture)
{
Debug.Assert(m_world.IsLocked == false);
if (m_world.IsLocked)
{
return;
}
Debug.Assert(fixture.Body == this);
// Remove the fixture from this body's singly linked list.
Debug.Assert(m_fixtureCount > 0);
b2Fixture node = m_fixtureList;
bool found = false;
while (node != null)
{
if (node == fixture)
{
node = fixture.Next;
found = true;
break;
}
node = node.Next;
}
// You tried to remove a shape that is not attached to this body.
Debug.Assert(found);
// Destroy any contacts associated with the fixture.
b2ContactEdge edge = m_contactList;
while (edge != null)
{
b2Contact c = edge.Contact;
edge = edge.Next;
b2Fixture fixtureA = c.FixtureA;
b2Fixture fixtureB = c.FixtureB;
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
m_world.ContactManager.Destroy(c);
}
}
if (m_flags.HasFlag(b2BodyFlags.e_activeFlag))
{
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
fixture.DestroyProxies(broadPhase);
}
fixture.Body = null;
fixture.Next = null;
--m_fixtureCount;
// Reset the mass data.
ResetMassData();
}
public override void SayGoodbye(b2Fixture fixture)
{
}
public b2Body(b2BodyDef bd, b2World world)
{
BodyFlags = 0;
if (bd.bullet)
{
BodyFlags |= b2BodyFlags.e_bulletFlag;
}
if (bd.fixedRotation)
{
BodyFlags |= b2BodyFlags.e_fixedRotationFlag;
}
if (bd.allowSleep)
{
BodyFlags |= b2BodyFlags.e_autoSleepFlag;
}
if (bd.awake)
{
BodyFlags |= b2BodyFlags.e_awakeFlag;
}
if (bd.active)
{
BodyFlags |= b2BodyFlags.e_activeFlag;
}
World = world;
Transform.p = bd.position;
Transform.q.Set(bd.angle);
Sweep.localCenter.SetZero();
Sweep.c0 = Transform.p;
Sweep.c = Transform.p;
Sweep.a0 = bd.angle;
Sweep.a = bd.angle;
Sweep.alpha0 = 0.0f;
JointList = null;
ContactList = null;
Prev = null;
Next = null;
m_linearVelocity = bd.linearVelocity;
m_angularVelocity = bd.angularVelocity;
LinearDamping = bd.linearDamping;
AngularDamping = bd.angularDamping;
GravityScale = bd.gravityScale;
Force.SetZero();
Torque = 0.0f;
SleepTime = 0.0f;
BodyType = bd.type;
if (BodyType == b2BodyType.b2_dynamicBody)
{
Mass = 1.0f;
InvertedMass = 1.0f;
}
else
{
Mass = 0.0f;
InvertedMass = 0.0f;
}
m_I = 0.0f;
InvertedI = 0.0f;
UserData = bd.userData;
FixtureList = null;
FixtureCount = 0;
}
public void AddPair(object proxyUserDataA, object proxyUserDataB)
{
b2FixtureProxy proxyA = (b2FixtureProxy)proxyUserDataA;
b2FixtureProxy proxyB = (b2FixtureProxy)proxyUserDataB;
b2Fixture fixtureA = proxyA.fixture;
b2Fixture fixtureB = proxyB.fixture;
int indexA = proxyA.childIndex;
int indexB = proxyB.childIndex;
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// TODO_ERIN use a hash table to remove a potential bottleneck when both
// bodies have a lot of contacts.
// Does a contact already exist?
b2ContactEdge edge = bodyB.GetContactList();
while (edge)
{
if (edge.other == bodyA)
{
b2Fixture fA = edge.contact.GetFixtureA();
b2Fixture fB = edge.contact.GetFixtureB();
int iA = edge.contact.GetChildIndexA();
int iB = edge.contact.GetChildIndexB();
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.next;
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if (m_contactFilter && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
b2Contact c = b2Contact.Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.GetFixtureA();
fixtureB = c.GetFixtureB();
indexA = c.GetChildIndexA();
indexB = c.GetChildIndexB();
bodyA = fixtureA.GetBody();
bodyB = fixtureB.GetBody();
// Insert into the world.
c.m_prev = null;
c.m_next = m_contactList;
if (m_contactList != null)
{
m_contactList.m_prev = c;
}
m_contactList = c;
// Connect to island graph.
// Connect to body A
c.m_nodeA.contact = c;
c.m_nodeA.other = bodyB;
c.m_nodeA.prev = null;
c.m_nodeA.next = bodyA.m_contactList;
if (bodyA.m_contactList != null)
{
bodyA.m_contactList.prev = &c.m_nodeA;
}
bodyA.m_contactList = &c.m_nodeA;
// Connect to body B
c.m_nodeB.contact = c;
c.m_nodeB.other = bodyA;
c.m_nodeB.prev = null;
c.m_nodeB.next = bodyB.m_contactList;
if (bodyB.m_contactList != null)
{
bodyB.m_contactList.prev = &c.m_nodeB;
}
bodyB.m_contactList = &c.m_nodeB;
// Wake up the bodies
bodyA.SetAwake(true);
bodyB.SetAwake(true);
++m_contactCount;
}
/// Return true if contact calculations should be performed between these two shapes. /// @warning for performance reasons this is only called when the AABBs begin to overlap. public abstract bool ShouldCollide(b2Fixture fixtureA, b2Fixture fixtureB);
public void SetFixtureName(b2Fixture fixture, String name)
{
m_fixtureToNameMap.Add(fixture, name);
}
public void SetCustomVector(b2Fixture item, String propertyName, b2Vec2 val)
{
m_fixturesWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_vec2.Add(propertyName, val);
}
public void SetCustomString(b2Fixture item, String propertyName, String val)
{
m_fixturesWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_string.Add(propertyName, val);
}
public void SetCustomFloat(b2Fixture item, String propertyName, float val)
{
m_fixturesWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_float.Add(propertyName, (float)val);
}
public String GetFixtureName(b2Fixture fixture)
{
if (m_fixtureToNameMap.ContainsKey(fixture))
return m_fixtureToNameMap[fixture];
return null;
}
public Car(
b2World b2world,
double width,
double length,
double[] position,
double angle,
double power,
double max_steer_angle,
double max_speed,
Wheel[] wheels
)
{
this.wheels = wheels;
// /*
// pars is an object with possible attributes:
// width - width of the car in meters
// length - length of the car in meters
// position - starting position of the car, array [x, y] in meters
// angle - starting angle of the car, degrees
// max_steer_angle - maximum angle the wheels turn when steering, degrees
// max_speed - maximum speed of the car, km/h
// power - engine force, in newtons, that is applied to EACH powered wheel
// wheels - wheel definitions: [{x, y, rotatable, powered}}, ...] where
// x is wheel position in meters relative to car body center
// y is wheel position in meters relative to car body center
// revolving - boolean, does this turn rotate when steering?
// powered - is force applied to this wheel when accelerating/braking?
// */
// this.max_steer_angle=pars.max_steer_angle;
// this.max_speed=pars.max_speed;
// this.power=pars.power;
var wheel_angle = 0.0;//keep track of current wheel angle relative to car.
// //when steering left/right, angle will be decreased/increased gradually over 200ms to prevent jerkyness.
//initialize body
var def = new b2BodyDef();
def.type = b2Body.b2_dynamicBody;
def.position = new b2Vec2(position[0], position[1]);
def.angle = angle.DegreesToRadians();
def.linearDamping = 0.55; //gradually reduces velocity, makes the car reduce speed slowly if neither accelerator nor brake is pressed
def.bullet = true; //dedicates more time to collision detection - car travelling at high speeds at low framerates otherwise might teleport through obstacles.
def.angularDamping = 0.3;
this.body = b2world.CreateBody(def);
//initialize shape
var fixdef = new b2FixtureDef();
fixdef.density = 1.0;
fixdef.friction = 0.3; //friction when rubbing agaisnt other shapes
fixdef.restitution = 0.4; //amount of force feedback when hitting something. >0 makes the car bounce off, it's fun!
var fixdef_shape = new b2PolygonShape();
fixdef.shape = fixdef_shape;
fixdef_shape.SetAsBox(width / 2, length / 2);
this.fix = body.CreateFixture(fixdef);
//initialize wheels
foreach (var item in wheels)
{
item.Initialize(this);
}
//return array of wheels that turn when steering
IEnumerable<Wheel> getRevolvingWheels = from w in wheels where w.revolving select w;
// //return array of powered wheels
IEnumerable<Wheel> getPoweredWheels = from w in wheels where w.powered select w;
#region setSpeed
Action<double> setSpeed = (speed) =>
{
/*
speed - speed in kilometers per hour
*/
var velocity0 = this.body.GetLinearVelocity();
//Console.WriteLine("car setSpeed velocity0 " + new { velocity0.x, velocity0.y });
var velocity2 = vectors.unit(new[] { velocity0.x, velocity0.y });
//Console.WriteLine("car setSpeed velocity2 " + new { x = velocity2[0], y = velocity2[1] });
var velocity = new b2Vec2(
velocity2[0] * ((speed * 1000.0) / 3600.0),
velocity2[1] * ((speed * 1000.0) / 3600.0)
);
//Console.WriteLine("car setSpeed SetLinearVelocity " + new { velocity.x, velocity.y });
this.body.SetLinearVelocity(velocity);
};
#endregion
#region getSpeedKMH
this.getSpeedKMH = delegate
{
var velocity = this.body.GetLinearVelocity();
var len = vectors.len(new double[] { velocity.x, velocity.y });
return (len / 1000.0) * 3600.0;
};
#endregion
#region getLocalVelocity
Func<double[]> getLocalVelocity = delegate
{
/*
returns car's velocity vector relative to the car
*/
var retv = this.body.GetLocalVector(this.body.GetLinearVelocityFromLocalPoint(new b2Vec2(0, 0)));
return new double[] { retv.x, retv.y };
};
#endregion
#region update
this.update = (msDuration) =>
{
#region 1. KILL SIDEWAYS VELOCITY
//kill sideways velocity for all wheels
for (var i = 0; i < wheels.Length; i++)
{
wheels[i].killSidewaysVelocity();
}
#endregion
#region 2. SET WHEEL ANGLE
//calculate the change in wheel's angle for this update, assuming the wheel will reach is maximum angle from zero in 200 ms
var incr = (max_steer_angle / 200.0) * msDuration;
if (steer_right == STEER_RIGHT)
{
wheel_angle = Math.Min(Math.Max(wheel_angle, 0) + incr, max_steer_angle); //increment angle without going over max steer
}
else if (steer_left == STEER_LEFT)
{
wheel_angle = Math.Max(Math.Min(wheel_angle, 0) - incr, -max_steer_angle); //decrement angle without going over max steer
}
else
{
wheel_angle = 0;
}
//update revolving wheels
getRevolvingWheels.WithEach(
w => w.setAngle(wheel_angle)
);
#endregion
#region 3. APPLY FORCE TO WHEELS
var base_vect = new double[2]; //vector pointing in the direction force will be applied to a wheel ; relative to the wheel.
//if accelerator is pressed down and speed limit has not been reached, go forwards
var lessthanlimit = (getSpeedKMH() < max_speed);
var flag1 = (accelerate == ACC_ACCELERATE) && lessthanlimit;
if (flag1)
{
base_vect = new double[] { 0, -1 };
}
else if (accelerate == ACC_BRAKE)
{
//braking, but still moving forwards - increased force
if (getLocalVelocity()[1] < 0)
{
base_vect = new double[] { 0, 1.3 };
}
//going in reverse - less force
else
{
base_vect = new double[] { 0, 0.7 };
}
}
else
{
base_vect[0] = 0;
base_vect[1] = 0;
}
//multiply by engine power, which gives us a force vector relative to the wheel
var fvect = new double[] {
power * base_vect[0],
power * base_vect[1]
};
//apply force to each wheel
getPoweredWheels.WithEachIndex(
(w, i) =>
{
var wp = w.body.GetWorldCenter();
var wf = w.body.GetWorldVector(new b2Vec2(fvect[0], fvect[1]));
//Console.WriteLine("getPoweredWheels ApplyForce #" + i);
w.body.ApplyForce(wf, wp);
}
);
//if going very slow, stop - to prevent endless sliding
var veryslow = (getSpeedKMH() < 4);
var flag2 = veryslow && (accelerate == ACC_NONE);
if (flag2)
{
//Console.WriteLine("setSpeed 0");
setSpeed(0);
}
#endregion
};
#endregion
}
/// <summary>
/// Called for each fixture found in the query. You control how the ray cast
/// proceeds by returning a float:
/// return -1: ignore this fixture and continue
/// return 0: terminate the ray cast
/// return fraction: clip the ray to this point
/// return 1: don't clip the ray and continue
/// </summary>
/// <param name="fixture">the fixture hit by the ray</param>
/// <param name="point">the point of initial intersection</param>
/// <param name="normal">the normal vector at the point of intersection</param>
/// <param name="fraction"></param>
/// <returns>-1 to filter, 0 to terminate, fraction to clip the ray for
/// closest hit, 1 to continue</returns>
public abstract float ReportFixture(b2Fixture fixture, b2Vec2 point,
b2Vec2 normal, float fraction);
/// Called when any fixture is about to be destroyed due /// to the destruction of its parent body. public abstract void SayGoodbye(b2Fixture fixture);
public virtual void DestroyFixture(b2Fixture fixture)
{
b2Assert(m_world.IsLocked() == false);
if (m_world.IsLocked() == true)
{
return;
}
b2Assert(fixture.m_body == this);
// Remove the fixture from this body's singly linked list.
b2Assert(m_fixtureCount > 0);
b2Fixture* node = &m_fixtureList;
bool found = false;
while (*node != null)
{
if (*node == fixture)
{
*node = fixture.Next;
found = true;
break;
}
node = &(*node).Next;
}
// You tried to remove a shape that is not attached to this body.
b2Assert(found);
// Destroy any contacts associated with the fixture.
b2ContactEdge* edge = m_contactList;
while (edge)
{
b2Contact* c = edge.contact;
edge = edge.next;
b2Fixture fixtureA = c.GetFixtureA();
b2Fixture fixtureB = c.GetFixtureB();
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
m_world.ContactManager.Destroy(c);
}
}
b2BlockAllocator* allocator = m_world.m_blockAllocator;
if (m_flags & e_activeFlag)
{
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
fixture.DestroyProxies(broadPhase);
}
fixture.Destroy(allocator);
fixture.m_body = null;
fixture.Next = null;
fixture.~b2Fixture();
allocator.Free(fixture, sizeof(b2Fixture));
--m_fixtureCount;
// Reset the mass data.
ResetMassData();
}
public void DestroyBody(b2Body b)
{
if (IsLocked())
{
return;
}
// Delete the attached joints.
b2JointEdge je = b.JointList;
while (je)
{
b2JointEdge je0 = je;
je = je.next;
if (m_destructionListener != null)
{
m_destructionListener.SayGoodbye(je0.joint);
}
DestroyJoint(je0.joint);
b.JointList = je;
}
b.JointList = null;
// Delete the attached contacts.
b2ContactEdge ce = b.ContactList;
while (ce)
{
b2ContactEdge ce0 = ce;
ce = ce.next;
m_contactManager.Destroy(ce0.contact);
}
b.ContactList = null;
// Delete the attached fixtures. This destroys broad-phase proxies.
b2Fixture f = b.FixtureList;
while (f != null)
{
b2Fixture f0 = f;
f = f.Next;
if (m_destructionListener != null)
{
m_destructionListener.SayGoodbye(f0);
}
f0.DestroyProxies(m_contactManager.BroadPhase);
b.FixtureList = f;
b.FixtureCount -= 1;
}
b.FixtureList = null;
b.FixtureCount = 0;
// Remove world body list.
if (b.Prev != null)
{
b.Prev.Next = b.Next;
}
if (b.Next != null)
{
b.Next.Prev = b.Prev;
}
if (b == m_bodyList)
{
m_bodyList = b.Next;
}
--m_bodyCount;
}
public virtual b2Fixture CreateFixture(b2FixtureDef def)
{
if (m_world.IsLocked == true)
{
return null;
}
b2Fixture fixture = new b2Fixture();
fixture.Create(this, def);
if (m_flags.HasFlag(b2BodyFlags.e_activeFlag))
{
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
fixture.CreateProxies(broadPhase, m_xf);
}
fixture.Next = m_fixtureList;
m_fixtureList = fixture;
++m_fixtureCount;
fixture.Body = this;
// Adjust mass properties if needed.
if (fixture.Density > 0.0f)
{
ResetMassData();
}
// Let the world know we have a new fixture. This will cause new contacts
// to be created at the beginning of the next time step.
m_world.Flags |= b2WorldFlags.e_newFixture;
return fixture;
}
// Find TOI contacts and solve them.
public void SolveTOI(b2TimeStep step)
{
b2Island island = new b2Island(2 * b2Settings.b2_maxTOIContacts, b2Settings.b2_maxTOIContacts, 0, m_contactManager.ContactListener);
if (m_stepComplete)
{
for (b2Body b = m_bodyList; b; b = b.Next)
{
b.BodyFlags &= ~b2Body.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
for (b2Contact c = m_contactManager.ContactList; c; c = c.Next)
{
// Invalidate TOI
c.ContactFlags &= ~(b2ContactType.e_toiFlag | b2ContactType.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (; ;)
{
// Find the first TOI.
b2Contact minContact = null;
float minAlpha = 1.0f;
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > b2Settings.b2_maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.ContactFlags.HasFlag(b2ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2BodyType typeA = bA.BodyType;
b2BodyType typeB = bB.BodyType;
bool activeA = bA.IsAwake() && typeA != b2BodyType.b2_staticBody;
bool activeB = bB.IsAwake() && typeB != b2BodyType.b2_staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != b2BodyType.b2_dynamicBody;
bool collideB = bB.IsBullet() || typeB != b2BodyType.b2_dynamicBody;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.Sweep.alpha0;
if (bA.Sweep.alpha0 < bB.Sweep.alpha0)
{
alpha0 = bB.Sweep.alpha0;
bA.Sweep.Advance(alpha0);
}
else if (bB.Sweep.alpha0 < bA.Sweep.alpha0)
{
alpha0 = bA.Sweep.alpha0;
bB.Sweep.Advance(alpha0);
}
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input = new b2TOIInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.sweepA = bA.Sweep;
input.sweepB = bB.Sweep;
input.tMax = 1.0f;
b2TOIOutput output = b2TimeOfImpact(input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == b2TOIOutputType.e_touching)
{
alpha = b2Math.b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.ContactFlags |= b2ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * b2Settings.b2_epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
{
// Advance the bodies to the TOI.
b2Fixture fA = minContact.GetFixtureA();
b2Fixture fB = minContact.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2Sweep backup1 = bA.Sweep;
b2Sweep backup2 = bB.Sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.ContactListener);
minContact.ContactFlags &= ~b2ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.Sweep = backup1;
bB.Sweep = backup2;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
continue;
}
bA.SetAwake(true);
bB.SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA.BodyFlags |= b2BodyFlags.e_islandFlag;
bB.BodyFlags |= b2BodyFlags.e_islandFlag;
minContact.ContentType |= b2ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
b2Body[] bodies = new b2Body[] { bA, bB };
for (int i = 0; i < 2; ++i)
{
b2Body body = bodies[i];
if (body.BodyType == b2BodyType.b2_dynamicBody)
{
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
if (island.BodyCount == island.BodyCapacity)
{
break;
}
if (island.ContactCount == island.ContactCapacity)
{
break;
}
b2Contact contact = ce.contact;
// Has this contact already been added to the island?
if (contact.ContactType & b2ContactType.e_islandFlag)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
b2Body other = ce.other;
if (other.BodyType == b2BodyType.b2_dynamicBody &&
body.IsBullet() == false && other.IsBullet() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
// Tentatively advance the body to the TOI.
b2Sweep backup = other.Sweep;
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(m_contactManager.ContactListener);
// Was the contact disabled by the user?
if (contact.IsEnabled() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact.IsTouching() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Add the contact to the island
contact.ContactFlags |= b2ContactFlags.e_islandFlag;
island.Add(contact);
// Has the other body already been added to the island?
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
continue;
}
// Add the other body to the island.
other.BodyFlags |= b2BodyFlags.e_islandFlag;
if (other.BodyType != b2BodyType.b2_staticBody)
{
other.SetAwake(true);
}
island.Add(other);
}
}
}
b2TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodyCount; ++i)
{
b2Body body = island.m_bodies[i];
body.BodyFlags &= ~b2BodyFlags.e_islandFlag;
if (body.BodyType != b2BodyType.b2_dynamicBody)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
ce.Contact.ContactFlags &= ~(b2ContactFlags.e_toiFlag | b2ContactFlags.e_islandFlag);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
}
protected b2Transform m_xf = b2Transform.Default; // the body origin transform
#endregion Fields
#region Constructors
public b2Body(b2BodyDef bd, b2World world)
{
m_flags = 0;
if (bd.bullet)
{
m_flags |= b2BodyFlags.e_bulletFlag;
}
if (bd.fixedRotation)
{
m_flags |= b2BodyFlags.e_fixedRotationFlag;
}
if (bd.allowSleep)
{
m_flags |= b2BodyFlags.e_autoSleepFlag;
}
if (bd.awake)
{
m_flags |= b2BodyFlags.e_awakeFlag;
}
if (bd.active)
{
m_flags |= b2BodyFlags.e_activeFlag;
}
m_world = world;
m_xf.p = bd.position;
m_xf.q.Set(bd.angle);
Sweep.localCenter.SetZero();
Sweep.c0 = m_xf.p;
Sweep.c = m_xf.p;
Sweep.a0 = bd.angle;
Sweep.a = bd.angle;
Sweep.alpha0 = 0.0f;
m_jointList = null;
m_contactList = null;
Prev = null;
Next = null;
m_linearVelocity = bd.linearVelocity;
m_angularVelocity = bd.angularVelocity;
m_linearDamping = bd.linearDamping;
m_angularDamping = bd.angularDamping;
m_gravityScale = bd.gravityScale;
m_force.SetZero();
m_torque = 0.0f;
m_sleepTime = 0.0f;
m_type = bd.type;
if (m_type == b2BodyType.b2_dynamicBody)
{
m_mass = 1.0f;
m_invMass = 1.0f;
}
else
{
m_mass = 0.0f;
m_invMass = 0.0f;
}
m_I = 0.0f;
m_invI = 0.0f;
m_userData = bd.userData;
m_fixtureList = null;
m_fixtureCount = 0;
}
public void Create(b2Body body, b2FixtureDef def)
{
UserData = def.userData;
Friction = def.friction;
Restitution = def.restitution;
Body = body;
Next = null;
m_filter = def.filter;
m_isSensor = def.isSensor;
Shape = def.shape.Clone();
// Reserve proxy space
int childCount = Shape.GetChildCount();
m_proxies = b2ArrayPool<b2FixtureProxy>.Create(childCount, true);
for (int i = 0; i < childCount; ++i)
{
m_proxies[i].fixture = null;
m_proxies[i].proxyId = b2BroadPhase.e_nullProxy;
}
m_proxyCount = 0;
Density = def.density;
}
public override float ReportFixture(b2Fixture fixture, b2Vec2 point,
b2Vec2 normal, float fraction)
{
m_fixture = fixture;
m_point = point;
m_normal = normal;
return fraction;
}
public virtual void ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
m_mass = 0.0f;
m_invMass = 0.0f;
m_I = 0.0f;
m_invI = 0.0f;
Sweep.localCenter.SetZero();
// Static and kinematic bodies have zero mass.
if (m_type == b2BodyType.b2_staticBody || m_type == b2BodyType.b2_kinematicBody)
{
Sweep.c0 = m_xf.p;
Sweep.c = m_xf.p;
Sweep.a0 = Sweep.a;
return;
}
Debug.Assert(m_type == b2BodyType.b2_dynamicBody);
// Accumulate mass over all fixtures.
b2Vec2 localCenter = b2Math.b2Vec2_zero;
for (b2Fixture f = m_fixtureList; f != null; f = f.Next)
{
if (f.Density == 0.0f)
{
continue;
}
b2MassData massData;
massData = f.GetMassData();
m_mass += massData.mass;
localCenter += massData.mass * massData.center;
m_I += massData.I;
}
// Compute center of mass.
if (m_mass > 0.0f)
{
m_invMass = 1.0f / m_mass;
localCenter *= m_invMass;
}
else
{
// Force all dynamic bodies to have a positive mass.
m_mass = 1.0f;
m_invMass = 1.0f;
}
if (m_I > 0.0f && (!m_flags.HasFlag(b2BodyFlags.e_fixedRotationFlag)))
{
// Center the inertia about the center of mass.
m_I -= m_mass * b2Math.b2Dot(localCenter, localCenter);
Debug.Assert(m_I > 0.0f);
m_invI = 1.0f / m_I;
}
else
{
m_I = 0.0f;
m_invI = 0.0f;
}
// Move center of mass.
b2Vec2 oldCenter = Sweep.c;
Sweep.localCenter = localCenter;
Sweep.c0 = Sweep.c = b2Math.b2Mul(m_xf, Sweep.localCenter);
// Update center of mass velocity.
m_linearVelocity += b2Math.b2Cross(m_angularVelocity, Sweep.c - oldCenter);
}
public void DrawDebugData()
{
if (m_debugDraw == null)
{
return;
}
b2DrawFlags flags = m_debugDraw.GetFlags();
if (flags & b2DrawFlags.e_shapeBit)
{
for (b2Body b = m_bodyList; b; b = b.Next)
{
b2Transform xf = b.Transform;
for (b2Fixture f = b.FixtureList; f != null; f = f.Next)
{
if (b.IsActive() == false)
{
DrawShape(f, xf, new b2Color(0.5f, 0.5f, 0.3f));
}
else if (b.GetType() == b2BodyType.b2_staticBody)
{
DrawShape(f, xf, new b2Color(0.5f, 0.9f, 0.5f));
}
else if (b.GetType() == b2BodyType.b2_kinematicBody)
{
DrawShape(f, xf, new b2Color(0.5f, 0.5f, 0.9f));
}
else if (b.IsAwake() == false)
{
DrawShape(f, xf, new b2Color(0.6f, 0.6f, 0.6f));
}
else
{
DrawShape(f, xf, new b2Color(0.9f, 0.7f, 0.7f));
}
}
}
}
if (flags.HasFlag(b2DrawFlags.e_jointBit))
{
for (b2Joint j = m_jointList; j != null; j = j.GetNext())
{
DrawJoint(j);
}
}
if (flags.HasFlag(b2DrawFlags.e_pairBit))
{
b2Color color = new b2Color(0.3f, 0.9f, 0.9f);
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
//b2Fixture fixtureA = c.GetFixtureA();
//b2Fixture fixtureB = c.GetFixtureB();
//b2Vec2 cA = fixtureA.GetAABB().GetCenter();
//b2Vec2 cB = fixtureB.GetAABB().GetCenter();
//m_debugDraw.DrawSegment(cA, cB, color);
}
}
if (flags.HasFlag(b2DrawFlags.e_aabbBit))
{
b2Color color(0.9f, 0.3f, 0.9f);
b2BroadPhase bp = m_contactManager.BroadPhase;
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
if (b.IsActive() == false)
{
continue;
}
for (b2Fixture f = b.FixtureList; f != null; f = f.Next)
{
for (int i = 0; i < f.ProxyCount; ++i)
{
b2FixtureProxy proxy = f.Proxies[i];
b2AABB aabb = bp.GetFatAABB(proxy.proxyId);
b2Vec2[] vs = new b2Vec2[4];
vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
m_debugDraw.DrawPolygon(vs, 4, color);
}
}
}
}
if (flags.HasFlag(b2DrawFlags.e_centerOfMassBit))
{
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
b2Transform xf = b.Transform;
xf.p = b.WorldCenter;
m_debugDraw.DrawTransform(xf);
}
}
}
protected void readCustomPropertiesFromJson(b2Fixture item, JObject value)
{
if (null == item)
return;
if (value["customProperties"] != null)
return;
int i = 0;
JArray propValues = (JArray)value["customProperties"];
if (null != propValues)
{
int numPropValues = propValues.Count;
for (i = 0; i < numPropValues; i++)
{
JObject propValue = (JObject)propValues[i];
String propertyName = propValue["name"].ToString();
if (propValue["int"] != null)
SetCustomInt(item, propertyName, (int)propValue["int"]);
if (propValue["float"] != null)
SetCustomFloat(item, propertyName, (float)propValue["float"]);
if (propValue["string"] != null)
SetCustomString(item, propertyName, propValue["string"].ToString());
if (propValue["vec2"] != null)
SetCustomVector(item, propertyName, this.jsonToVec("vec2", propValue));
if (propValue["bool"] != null)
SetCustomBool(item, propertyName, (bool)propValue["bool"]);
}
}
}
private void DrawShape(b2Fixture shape, b2Transform xf, b2Color color);
public QueryCallback(b2Vec2 point)
{
m_point = point;
m_fixture = null;
}
/// Called when any fixture is about to be destroyed due /// to the destruction of its parent body. public abstract void SayGoodbye(b2Fixture fixture);
private void DrawShape(b2Fixture shape, b2Transform xf, b2Color color);
public b2Body(b2BodyDef bd, b2World world)
{
m_flags = 0;
if (bd.bullet)
{
m_flags |= b2BodyFlags.e_bulletFlag;
}
if (bd.fixedRotation)
{
m_flags |= b2BodyFlags.e_fixedRotationFlag;
}
if (bd.allowSleep)
{
m_flags |= b2BodyFlags.e_autoSleepFlag;
}
if (bd.awake)
{
m_flags |= b2BodyFlags.e_awakeFlag;
}
if (bd.active)
{
m_flags |= b2BodyFlags.e_activeFlag;
}
m_world = world;
m_xf.p = bd.position;
m_xf.q.Set(bd.angle);
Sweep.localCenter.SetZero();
Sweep.c0 = m_xf.p;
Sweep.c = m_xf.p;
Sweep.a0 = bd.angle;
Sweep.a = bd.angle;
Sweep.alpha0 = 0.0f;
m_jointList = null;
m_contactList = null;
Prev = null;
Next = null;
m_linearVelocity = bd.linearVelocity;
m_angularVelocity = bd.angularVelocity;
m_linearDamping = bd.linearDamping;
m_angularDamping = bd.angularDamping;
m_gravityScale = bd.gravityScale;
m_force.SetZero();
m_torque = 0.0f;
m_sleepTime = 0.0f;
m_type = bd.type;
if (m_type == b2BodyType.b2_dynamicBody)
{
m_mass = 1.0f;
m_invMass = 1.0f;
}
else
{
m_mass = 0.0f;
m_invMass = 0.0f;
}
m_I = 0.0f;
m_invI = 0.0f;
m_userData = bd.userData;
m_fixtureList = null;
m_fixtureCount = 0;
}
public void Create(b2Body body, b2FixtureDef def)
{
m_userData = def.userData;
m_friction = def.friction;
m_restitution = def.restitution;
m_body = body;
m_next = null;
m_filter = def.filter;
m_isSensor = def.isSensor;
m_shape = def.shape.Clone();
// Reserve proxy space
int childCount = m_shape.GetChildCount();
for (int i = 0; i < childCount; ++i)
{
b2FixtureProxy proxy = new b2FixtureProxy();
proxy.fixture = null;
proxy.proxyId = b2BroadPhase.e_nullProxy;
m_proxies.Add(proxy);
}
m_proxyCount = 0;
m_density = def.density;
}
public virtual void DestroyFixture(b2Fixture fixture)
{
Debug.Assert(m_world.IsLocked == false);
if (m_world.IsLocked)
{
return;
}
Debug.Assert(fixture.Body == this);
// Remove the fixture from this body's singly linked list.
Debug.Assert(m_fixtureCount > 0);
b2Fixture node = m_fixtureList;
bool found = false;
while (node != null)
{
if (node == fixture)
{
node = fixture.Next;
found = true;
break;
}
node = node.Next;
}
// You tried to remove a shape that is not attached to this body.
Debug.Assert(found);
// Destroy any contacts associated with the fixture.
b2ContactEdge edge = m_contactList;
while (edge != null)
{
b2Contact c = edge.Contact;
edge = edge.Next;
b2Fixture fixtureA = c.FixtureA;
b2Fixture fixtureB = c.FixtureB;
if (fixture == fixtureA || fixture == fixtureB)
{
// This destroys the contact and removes it from
// this body's contact list.
m_world.ContactManager.Destroy(c);
}
}
if (m_flags.HasFlag(b2BodyFlags.e_activeFlag))
{
b2BroadPhase broadPhase = m_world.ContactManager.BroadPhase;
fixture.DestroyProxies(broadPhase);
}
fixture.Body = null;
fixture.Next = null;
--m_fixtureCount;
// Reset the mass data.
ResetMassData();
}
public Wheel()
{
this.Initialize = car =>
{
/*
wheel object
pars:
car - car this wheel belongs to
x - horizontal position in meters relative to car's center
y - vertical position in meters relative to car's center
width - width in meters
length - length in meters
revolving - does this wheel revolve when steering?
powered - is this wheel powered?
*/
var position = new double[] { x, y };
//this.car=pars.car;
//this.revolving=pars.revolving;
//this.powered=pars.powered;
//initialize body
var def = new b2BodyDef();
def.type = b2Body.b2_dynamicBody;
def.position = car.body.GetWorldPoint(new b2Vec2(position[0], position[1]));
def.angle = car.body.GetAngle();
this.body = b2world.CreateBody(def);
//initialize shape
var fixdef = new b2FixtureDef();
fixdef.density = 1;
fixdef.isSensor = true; //wheel does not participate in collision calculations: resulting complications are unnecessary
var fixdef_shape = new b2PolygonShape();
fixdef.shape = fixdef_shape;
fixdef_shape.SetAsBox(width / 2, length / 2);
this.fix = body.CreateFixture(fixdef);
var jointdef = new b2RevoluteJointDef();
//create joint to connect wheel to body
if (revolving)
{
jointdef.Initialize(car.body, body, body.GetWorldCenter());
jointdef.enableMotor = false; //we'll be controlling the wheel's angle manually
}
else
{
jointdef.Initialize(car.body, body, body.GetWorldCenter()
//, new b2Vec2(1, 0)
);
jointdef.enableLimit = true;
//jointdef.lowerTranslation = 0;
//jointdef.upperTranslation = 0;
}
joint = b2world.CreateJoint(jointdef);
#region setAngle
this.setAngle +=
(angle) =>
{
this.rotation = angle.DegreesToRadians();
/*
angle - wheel angle relative to car, in degrees
*/
body.SetAngle(car.body.GetAngle() + angle.DegreesToRadians());
};
#endregion
#region getLocalVelocity
Func<double[]> getLocalVelocity = delegate
{
/*returns get velocity vector relative to car
*/
var res = car.body.GetLocalVector(car.body.GetLinearVelocityFromLocalPoint(new b2Vec2(position[0], position[1])));
return new double[] { res.x, res.y };
};
#endregion
#region getDirectionVector
Func<double[]> getDirectionVector = delegate
{
/*
returns a world unit vector pointing in the direction this wheel is moving
*/
if (getLocalVelocity()[1] > 0)
return vectors.rotate(new double[] { 0, 1 }, body.GetAngle());
else
return vectors.rotate(new double[] { 0, -1 }, body.GetAngle());
};
#endregion
#region getKillVelocityVector
Func<double[]> getKillVelocityVector = delegate
{
/*
substracts sideways velocity from this wheel's velocity vector and returns the remaining front-facing velocity vector
*/
var velocity = body.GetLinearVelocity();
var sideways_axis = getDirectionVector();
var dotprod = vectors.dot(new[] { velocity.x, velocity.y }, sideways_axis);
return new double[] { sideways_axis[0] * dotprod, sideways_axis[1] * dotprod };
};
#endregion
#region killSidewaysVelocity
this.killSidewaysVelocity = delegate
{
/*
removes all sideways velocity from this wheels velocity
*/
var kv = getKillVelocityVector();
body.SetLinearVelocity(new b2Vec2(kv[0], kv[1]));
};
#endregion
};
}
public void Destroy(b2Contact c)
{
b2Fixture fixtureA = c.GetFixtureA();
b2Fixture fixtureB = c.GetFixtureB();
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
if (m_contactListener && c.IsTouching())
{
m_contactListener.EndContact(c);
}
// Remove from the world.
if (c.m_prev)
{
c.m_prev.m_next = c.m_next;
}
if (c.m_next)
{
c.m_next.m_prev = c.m_prev;
}
if (c == m_contactList)
{
m_contactList = c.m_next;
}
// Remove from body 1
if (c.m_nodeA.prev)
{
c.m_nodeA.prev.next = c.m_nodeA.next;
}
if (c.m_nodeA.next)
{
c.m_nodeA.next.prev = c.m_nodeA.prev;
}
if (c.m_nodeA == bodyA.m_contactList)
{
bodyA.m_contactList = c.m_nodeA.next;
}
// Remove from body 2
if (c.m_nodeB.prev)
{
c.m_nodeB.prev.next = c.m_nodeB.next;
}
if (c.m_nodeB.next)
{
c.m_nodeB.next.prev = c.m_nodeB.prev;
}
if (c.m_nodeB == bodyB.m_contactList)
{
bodyB.m_contactList = c.m_nodeB.next;
}
// Call the factory.
b2Contact.Destroy(c);
--m_contactCount;
}
/// Called when any fixture is about to be destroyed due
/// to the destruction of its parent body.
public override void SayGoodbye(b2Fixture fixture)
{
Console.WriteLine("ByeBye Fixture: {0}", fixture);
}
/**
* Called when any fixture is about to be destroyed due
* to the destruction of its parent body.
*/
public void SayGoodbyeFixture(b2Fixture fixture)
{
}
/// Return true if contact calculations should be performed between these two shapes. /// @warning for performance reasons this is only called when the AABBs begin to overlap. public abstract bool ShouldCollide(b2Fixture fixtureA, b2Fixture fixtureB);
