public PolygonShape()
{
ShapeType = ShapeType.Polygon;
Radius = Settings.PolygonRadius;
Count = 0;
Centroid.SetZero();
}
internal WheelJoint(WheelJointDef def) : base(def)
{
_localAnchorA = def.LocalAnchorA;
_localAnchorB = def.LocalAnchorB;
_localXAxisA = def.LocalAxisA;
_localYAxisA = MathUtils.Cross(1.0f, _localXAxisA);
_mass = 0.0f;
_impulse = 0.0f;
_motorMass = 0.0f;
_motorImpulse = 0.0f;
_springMass = 0.0f;
_springImpulse = 0.0f;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableMotor = def.EnableMotor;
_frequencyHz = def.FrequencyHz;
_dampingRatio = def.DampingRatio;
_bias = 0.0f;
_gamma = 0.0f;
_ax.SetZero();
_ay.SetZero();
}
public FrictionJointDef()
{
JointType = JointType.FrictionJoint;
LocalAnchorA.SetZero();
LocalAnchorB.SetZero();
MaxForce = 0.0f;
MaxTorque = 0.0f;
}
public MotorJointDef()
{
JointType = JointType.MotorJoint;
LinearOffset.SetZero();
AngularOffset = 0.0f;
MaxForce = 1.0f;
MaxTorque = 1.0f;
CorrectionFactor = 0.3f;
}
internal FrictionJoint(FrictionJointDef def) : base(def)
{
_localAnchorA = def.LocalAnchorA;
_localAnchorB = def.LocalAnchorB;
_linearImpulse.SetZero();
_angularImpulse = 0.0f;
_maxForce = def.MaxForce;
_maxTorque = def.MaxTorque;
}
internal MotorJoint(MotorJointDef def) : base(def)
{
_linearOffset = def.LinearOffset;
_angularOffset = def.AngularOffset;
_linearImpulse.SetZero();
_angularImpulse = 0.0f;
_maxForce = def.MaxForce;
_maxTorque = def.MaxTorque;
_correctionFactor = def.CorrectionFactor;
}
public WheelJointDef()
{
JointType = JointType.WheelJoint;
LocalAnchorA.SetZero();
LocalAnchorB.SetZero();
LocalAxisA.Set(1.0f, 0.0f);
EnableMotor = false;
MaxMotorTorque = 0.0f;
MotorSpeed = 0.0f;
FrequencyHz = 2.0f;
DampingRatio = 0.7f;
}
private PrismaticJointDef()
{
JointType = JointType.PrismaticJoint;
LocalAnchorA.SetZero();
LocalAnchorB.SetZero();
LocalAxisA.Set(1.0f, 0.0f);
ReferenceAngle = 0.0f;
EnableLimit = false;
LowerTranslation = 0.0f;
UpperTranslation = 0.0f;
EnableMotor = false;
MaxMotorForce = 0.0f;
MotorSpeed = 0.0f;
}
internal MouseJoint(MouseJointDef def) : base(def)
{
Debug.Assert(def.Target.IsValid());
Debug.Assert(def.MaxForce.IsValid() && def.MaxForce >= 0.0f);
Debug.Assert(def.FrequencyHz.IsValid() && def.FrequencyHz >= 0.0f);
Debug.Assert(def.DampingRatio.IsValid() && def.DampingRatio >= 0.0f);
_targetA = def.Target;
_localAnchorB = MathUtils.MulT(BodyB.GetTransform(), _targetA);
_maxForce = def.MaxForce;
_impulse.SetZero();
_frequencyHz = def.FrequencyHz;
_dampingRatio = def.DampingRatio;
_beta = 0.0f;
_gamma = 0.0f;
}
internal PrismaticJoint(PrismaticJointDef def) : base(def)
{
LocalAnchorA = def.LocalAnchorA;
LocalAnchorB = def.LocalAnchorB;
LocalXAxisA = def.LocalAxisA;
LocalXAxisA.Normalize();
_localYAxisA = MathUtils.Cross(1.0f, LocalXAxisA);
ReferenceAngle = def.ReferenceAngle;
_impulse.SetZero();
_motorMass = 0.0f;
_motorImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
_maxMotorForce = def.MaxMotorForce;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_limitState = LimitState.InactiveLimit;
_axis.SetZero();
_perp.SetZero();
}
/// Create a chain with isolated end vertices.
/// @param vertices an array of vertices, these are copied
/// @param count the vertex count
public void CreateChain(Vector2[] vertices)
{
var count = vertices.Length;
Debug.Assert(Vertices == null && Count == 0);
Debug.Assert(count >= 2);
for (var i = 1; i < count; ++i)
{
// If the code crashes here, it means your vertices are too close together.
Debug.Assert(
MathUtils.DistanceSquared(vertices[i - 1], vertices[i])
> Settings.LinearSlop * Settings.LinearSlop);
}
Count = count;
Vertices = new Vector2[count];
Array.Copy(vertices, Vertices, count);
HasPrevVertex = false;
HasNextVertex = false;
PrevVertex.SetZero();
NextVertex.SetZero();
}
public bool RayCast(out RayCastOutput output, RayCastInput input)
{
output = default;
var tmin = -Settings.MaxFloat;
var tmax = Settings.MaxFloat;
var p = input.P1;
var d = input.P2 - input.P1;
var absD = Vector2.Abs(d);
var normal = new Vector2();
{
if (absD.X < Settings.Epsilon)
{
// Parallel.
if (p.X < LowerBound.X || UpperBound.X < p.X)
{
return(false);
}
}
else
{
var invD = 1.0f / d.X;
var t1 = (LowerBound.X - p.X) * invD;
var t2 = (UpperBound.X - p.X) * invD;
// Sign of the normal vector.
var s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.X = s;
tmin = t1;
}
// Pull the max down
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
}
}
{
if (absD.Y < Settings.Epsilon)
{
// Parallel.
if (p.Y < LowerBound.Y || UpperBound.Y < p.Y)
{
return(false);
}
}
else
{
var invD = 1.0f / d.Y;
var t1 = (LowerBound.Y - p.Y) * invD;
var t2 = (UpperBound.Y - p.Y) * invD;
// Sign of the normal vector.
var s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.Y = s;
tmin = t1;
}
// Pull the max down
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.MaxFraction < tmin)
{
return(false);
}
// Intersection.
output = new RayCastOutput {
Fraction = tmin, Normal = normal
};
return(true);
}
public GearJoint(GearJointDef def) : base(def)
{
_joint1 = def.Joint1;
_joint2 = def.Joint2;
_typeA = _joint1.JointType;
_typeB = _joint2.JointType;
Debug.Assert(_typeA == JointType.RevoluteJoint || _typeA == JointType.PrismaticJoint);
Debug.Assert(_typeB == JointType.RevoluteJoint || _typeB == JointType.PrismaticJoint);
Single coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = _joint1.BodyA;
BodyA = _joint1.BodyB;
// Get geometry of joint1
var xfA = BodyA.Transform;
var aA = BodyA.Sweep.A;
var xfC = _bodyC.Transform;
var aC = _bodyC.Sweep.A;
if (_typeA == JointType.RevoluteJoint)
{
var revolute = (RevoluteJoint)def.Joint1;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC.SetZero();
coordinateA = aA - aC - _referenceAngleA;
}
else
{
var prismatic = (PrismaticJoint)def.Joint1;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxisA;
var pC = _localAnchorC;
var pA = MathUtils.MulT(
xfC.Rotation,
MathUtils.Mul(xfA.Rotation, _localAnchorA) + (xfA.Position - xfC.Position));
coordinateA = MathUtils.Dot(pA - pC, _localAxisC);
}
_bodyD = _joint2.BodyA;
BodyB = _joint2.BodyB;
// Get geometry of joint2
var xfB = BodyB.Transform;
var aB = BodyB.Sweep.A;
var xfD = _bodyD.Transform;
var aD = _bodyD.Sweep.A;
if (_typeB == JointType.RevoluteJoint)
{
var revolute = (RevoluteJoint)def.Joint2;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD.SetZero();
coordinateB = aB - aD - _referenceAngleB;
}
else
{
var prismatic = (PrismaticJoint)def.Joint2;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxisA;
var pD = _localAnchorD;
var pB = MathUtils.MulT(
xfD.Rotation,
MathUtils.Mul(xfB.Rotation, _localAnchorB) + (xfB.Position - xfD.Position));
coordinateB = MathUtils.Dot(pB - pD, _localAxisD);
}
_ratio = def.Ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
internal Transform Transform; // the body origin transform
internal Body(BodyDef def, World world)
{
Debug.Assert(def.Position.IsValid());
Debug.Assert(def.LinearVelocity.IsValid());
Debug.Assert(def.Angle.IsValid());
Debug.Assert(def.AngularVelocity.IsValid());
Debug.Assert(def.AngularDamping.IsValid() && def.AngularDamping >= 0.0f);
Debug.Assert(def.LinearDamping.IsValid() && def.LinearDamping >= 0.0f);
Flags = 0;
if (def.Bullet)
{
Flags |= BodyFlags.IsBullet;
}
if (def.FixedRotation)
{
Flags |= BodyFlags.FixedRotation;
}
if (def.AllowSleep)
{
Flags |= BodyFlags.AutoSleep;
}
if (def.Awake)
{
Flags |= BodyFlags.IsAwake;
}
if (def.Active)
{
Flags |= BodyFlags.IsActive;
}
_world = world;
Transform.Position = def.Position;
Transform.Rotation.Set(def.Angle);
Sweep = new Sweep
{
LocalCenter = Vector2.Zero,
C0 = Transform.Position,
C = Transform.Position,
A0 = def.Angle,
A = def.Angle,
Alpha0 = 0.0f
};
JointEdges = new LinkedList <JointEdge>();
ContactEdges = new LinkedList <ContactEdge>();
Fixtures = new List <Fixture>();
Node = null;
LinearVelocity = def.LinearVelocity;
AngularVelocity = def.AngularVelocity;
_linearDamping = def.LinearDamping;
AngularDamping = def.AngularDamping;
GravityScale = def.GravityScale;
Force.SetZero();
Torque = 0.0f;
SleepTime = 0.0f;
_type = def.BodyType;
if (_type == BodyType.DynamicBody)
{
_mass = 1.0f;
InvMass = 1.0f;
}
else
{
_mass = 0.0f;
InvMass = 0.0f;
}
_inertia = 0.0f;
InverseInertia = 0.0f;
UserData = def.UserData;
}
public CircleShape()
{
ShapeType = ShapeType.Circle;
Radius = 0.0f;
Position.SetZero();
}