public WeldJointDef()
{
type = JointType.WELD;
localAnchorA = new Vec2();
localAnchorB = new Vec2();
referenceAngle = 0.0f;
}
/// <summary>
/// Initialize the bodies, anchors, axis, and reference angle using the world anchor and world axis.
/// </summary>
public virtual void initialize(Body bA, Body bB, Vec2 anchor)
{
bodyA = bA;
bodyB = bB;
bA.getLocalPointToOut(anchor, localAnchorA);
bB.getLocalPointToOut(anchor, localAnchorB);
}
public ContactPositionConstraint()
{
for (int i = 0; i < LocalPoints.Length; i++)
{
LocalPoints[i] = new Vec2();
}
}
/// <summary>
/// Initialize the bodies, anchors, axis, and reference angle using the world anchor and world axis.
/// </summary>
public void Initialize(Body bA, Body bB, Vec2 anchor)
{
BodyA = bA;
BodyB = bB;
bA.GetLocalPointToOut(anchor, LocalAnchorA);
bB.GetLocalPointToOut(anchor, LocalAnchorB);
}
public WeldJointDef()
{
Type = JointType.Weld;
LocalAnchorA = new Vec2();
LocalAnchorB = new Vec2();
ReferenceAngle = 0.0f;
}
public FrictionJointDef()
{
Type = JointType.Friction;
LocalAnchorA = new Vec2();
LocalAnchorB = new Vec2();
MaxForce = 0f;
MaxTorque = 0f;
}
public FrictionJointDef()
{
type = JointType.FRICTION;
localAnchorA = new Vec2();
localAnchorB = new Vec2();
maxForce = 0f;
maxTorque = 0f;
}
protected internal Vec2[] GetInitializedArray(int argLength)
{
var ray = new Vec2[argLength];
for (int i = 0; i < ray.Length; i++)
{
ray[i] = new Vec2();
}
return ray;
}
public WorldManifold()
{
normal = new Vec2();
points = new Vec2[Settings.maxManifoldPoints];
for (int i = 0; i < Settings.maxManifoldPoints; i++)
{
points[i] = new Vec2();
}
}
public WorldManifold()
{
Normal = new Vec2();
Points = new Vec2[Settings.MAX_MANIFOLD_POINTS];
for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; i++)
{
Points[i] = new Vec2();
}
}
/// <summary> creates a manifold with 0 points, with it's points array full of instantiated ManifoldPoints.</summary>
public Manifold()
{
points = new ManifoldPoint[Settings.maxManifoldPoints];
for (int i = 0; i < Settings.maxManifoldPoints; i++)
{
points[i] = new ManifoldPoint();
}
localNormal = new Vec2();
localPoint = new Vec2();
pointCount = 0;
}
public PulleyJointDef()
{
Type = JointType.Pulley;
GroundAnchorA = new Vec2(-1.0f, 1.0f);
GroundAnchorB = new Vec2(1.0f, 1.0f);
LocalAnchorA = new Vec2(-1.0f, 0.0f);
LocalAnchorB = new Vec2(1.0f, 0.0f);
LengthA = 0.0f;
LengthB = 0.0f;
Ratio = 1.0f;
CollideConnected = true;
}
/// <param name="argWorldPool"></param>
/// <param name="def"></param>
public FrictionJoint(IWorldPool argWorldPool, FrictionJointDef def)
: base(argWorldPool, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_linearImpulse = new Vec2();
m_angularImpulse = 0.0f;
m_maxForce = def.maxForce;
m_maxTorque = def.maxTorque;
}
public PulleyJointDef()
{
type = JointType.PULLEY;
groundAnchorA = new Vec2(-1.0f, 1.0f);
groundAnchorB = new Vec2(1.0f, 1.0f);
localAnchorA = new Vec2(-1.0f, 0.0f);
localAnchorB = new Vec2(1.0f, 0.0f);
lengthA = 0.0f;
lengthB = 0.0f;
ratio = 1.0f;
collideConnected = true;
}
public PolygonShape()
: base(ShapeType.Polygon)
{
VertexCount = 0;
Vertices = new Vec2[Settings.MAX_POLYGON_VERTICES];
for (int i = 0; i < Vertices.Length; i++)
{
Vertices[i] = new Vec2();
}
Normals = new Vec2[Settings.MAX_POLYGON_VERTICES];
for (int i = 0; i < Normals.Length; i++)
{
Normals[i] = new Vec2();
}
Radius = Settings.POLYGON_RADIUS;
Centroid.SetZero();
}
static void Main(string[] args)
{
// Static Body
Vec2 gravity = new Vec2(0, -10);
bool doSleep = true;
World world = new World(gravity);
world.SleepingAllowed = doSleep;
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.position.set_Renamed(0, -10);
Body groundBody = world.createBody(groundBodyDef);
PolygonShape groundBox = new PolygonShape();
groundBox.setAsBox(50, 10);
groundBody.createFixture(groundBox, 0);
// Dynamic Body
BodyDef bodyDef = new BodyDef();
bodyDef.type = BodyType.DYNAMIC;
bodyDef.position.set_Renamed(0, 4);
Body body = world.createBody(bodyDef);
PolygonShape dynamicBox = new PolygonShape();
dynamicBox.setAsBox(1, 1);
FixtureDef fixtureDef = new FixtureDef();
fixtureDef.shape = dynamicBox;
fixtureDef.density = 1;
fixtureDef.friction = 0.3f;
body.createFixture(fixtureDef);
// Setup world
float timeStep = 1.0f / 60.0f;
int velocityIterations = 6;
int positionIterations = 2;
// Run loop
for (int i = 0; i < 60; ++i)
{
world.step(timeStep, velocityIterations, positionIterations);
Vec2 position = body.Position;
float angle = body.Angle;
Console.WriteLine("{0:0.00} {1:0.00} {2:0.00}", position.x, position.y, angle);
}
}
public override void getAnchorB(Vec2 argOut)
{
m_bodyB.getWorldPointToOut(m_localAnchorB, argOut);
}
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
: base(argWorld, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_localXAxisA = new Vec2(def.localAxisA);
m_localXAxisA.normalize();
m_localYAxisA = new Vec2();
Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse = new Vec3();
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_lowerTranslation = def.lowerTranslation;
m_upperTranslation = def.upperTranslation;
m_maxMotorForce = def.maxMotorForce;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
m_K = new Mat33();
m_axis = new Vec2();
m_perp = new Vec2();
}
public override void getReactionForce(float inv_dt, Vec2 argOut)
{
argOut.set_Renamed(m_uB).mulLocal(m_impulse).mulLocal(inv_dt);
}
/// <summary>
/// Ray-cast the world for all fixtures in the path of the ray. Your callback controls whether you
/// get the closest point, any point, or n-points. The ray-cast ignores shapes that contain the
/// starting point.
/// </summary>
/// <param name="callback">a user implemented callback class.</param>
/// <param name="point1">the ray starting point</param>
/// <param name="point2">the ray ending point</param>
public virtual void raycast(RayCastCallback callback, Vec2 point1, Vec2 point2)
{
wrcwrapper.broadPhase = m_contactManager.m_broadPhase;
wrcwrapper.callback = callback;
input.maxFraction = 1.0f;
input.p1.set_Renamed(point1);
input.p2.set_Renamed(point2);
m_contactManager.m_broadPhase.raycast(wrcwrapper, input);
}
/// <summary>
/// Construct a world object.
/// </summary>
/// <param name="gravity">the world gravity vector.</param>
/// <param name="doSleep">improve performance by not simulating inactive bodies.</param>
public World(Vec2 gravity, IWorldPool argPool)
{
contactStacks = new ContactRegister[ShapeTypesCount][];
for (int i = 0; i < ShapeTypesCount; i++)
{
contactStacks[i] = new ContactRegister[ShapeTypesCount];
}
pool = argPool;
m_destructionListener = null;
m_debugDraw = null;
m_bodyList = null;
m_jointList = null;
m_bodyCount = 0;
m_jointCount = 0;
m_warmStarting = true;
m_continuousPhysics = true;
m_subStepping = false;
m_stepComplete = true;
m_allowSleep = true;
m_gravity.set_Renamed(gravity);
m_flags = CLEAR_FORCES;
m_inv_dt0 = 0f;
m_contactManager = new ContactManager(this);
m_profile = new Profile();
initializeRegisters();
}
private Body[] stack = new Body[10]; // TODO djm find a good initial stack number;
#endregion Fields
#region Constructors
public World(Vec2 gravity)
: this(gravity, new DefaultWorldPool(WORLD_POOL_SIZE, WORLD_POOL_CONTAINER_SIZE))
{
}
public PrismaticJointDef()
{
type = JointType.PRISMATIC;
localAnchorA = new Vec2();
localAnchorB = new Vec2();
localAxisA = new Vec2(1.0f, 0.0f);
referenceAngle = 0.0f;
enableLimit = false;
lowerTranslation = 0.0f;
upperTranslation = 0.0f;
enableMotor = false;
maxMotorForce = 0.0f;
motorSpeed = 0.0f;
}
public void getLocalVectorToOutUnsafe(Vec2 worldVector, Vec2 out_Renamed)
{
Rot.mulTransUnsafe(m_xf.q, worldVector, out_Renamed);
}
/// <summary>
/// Gets a local vector given a world vector.
/// </summary>
/// <param name="a">vector in world coordinates.</param>
/// <returns>the corresponding local vector.</returns>
public Vec2 getLocalVector(Vec2 worldVector)
{
Vec2 out_Renamed = new Vec2();
getLocalVectorToOut(worldVector, out_Renamed);
return out_Renamed;
}
public override void getReactionForce(float inv_dt, Vec2 argOut)
{
Vec2 temp = pool.popVec2();
temp.set_Renamed(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
argOut.set_Renamed(m_perp).mulLocal(m_impulse.x).addLocal(temp).mulLocal(inv_dt);
pool.pushVec2(1);
}
/// <summary>
/// Call MoveProxy as many times as you like, then when you are done call UpdatePairs to finalized
/// the proxy pairs (for your time step).
/// </summary>
public void MoveProxy(int proxyId, AABB aabb, Vec2 displacement)
{
bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement);
if (buffer)
{
BufferMove(proxyId);
}
}
/// <summary>
/// Initialize the bodies, anchors, axis, and reference angle using the world anchor and world axis.
/// </summary>
public virtual void initialize(Body b1, Body b2, Vec2 anchor, Vec2 axis)
{
bodyA = b1;
bodyB = b2;
bodyA.getLocalPointToOut(anchor, localAnchorA);
bodyB.getLocalPointToOut(anchor, localAnchorB);
bodyA.getLocalVectorToOut(axis, localAxisA);
referenceAngle = bodyB.Angle - bodyA.Angle;
}
/// <summary>
/// Initialize the bodies, anchors, and reference angle using a world anchor point.
/// </summary>
/// <param name="bA"></param>
/// <param name="bB"></param>
/// <param name="anchor"></param>
public virtual void initialize(Body bA, Body bB, Vec2 anchor)
{
bodyA = bA;
bodyB = bB;
bodyA.getLocalPointToOut(anchor, localAnchorA);
bodyB.getLocalPointToOut(anchor, localAnchorB);
referenceAngle = bodyB.Angle - bodyA.Angle;
}
public void getLocalPointToOut(Vec2 worldPoint, Vec2 out_Renamed)
{
Transform.mulTransToOut(m_xf, worldPoint, out_Renamed);
}