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;
_axialMass = 0.0f;
_lowerImpulse = 0.0f;
_upperImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
_enableLimit = def.EnableLimit;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableMotor = def.EnableMotor;
_bias = 0.0f;
_gamma = 0.0f;
_ax.SetZero();
_ay.SetZero();
_stiffness = def.Stiffness;
_damping = def.Damping;
}
private void CreatePolygon()
{
DeusaldSharp.Vector2[] points = new DeusaldSharp.Vector2[_Points.Length];
for (int i = 0; i < points.Length; ++i)
{
points[i] = ToVec2(_Points[i]);
}
if (_IsObstacle)
{
NavElement element = Nav2D.AddObstacle(points, ToVec2(_SpawnPosition), _SpawnRotation, _ExtraOffset);
GameObject elementObject = Instantiate(_NavMeshElementObject, _SpawnPosition, Quaternion.identity);
elementObject.GetComponent <NavMeshElement>().Init(element, this);
}
else
{
if (_Cost <= 0f)
{
Debug.LogError("Cost can't be less than 0");
return;
}
NavElement element = Nav2D.AddSurface(points, ToVec2(_SpawnPosition), _SpawnRotation, _Cost, _ExtraOffset);
GameObject elementObject = Instantiate(_NavMeshElementObject, _SpawnPosition, Quaternion.identity);
elementObject.GetComponent <NavMeshElement>().Init(element, this);
}
}
/// Create a loop. This automatically adjusts connectivity.
/// @param vertices an array of vertices, these are copied
/// @param count the vertex count
public void CreateLoop(Vector2[] vertices, int count = -1)
{
if (count == -1)
{
count = vertices.Length;
}
Debug.Assert(Vertices == null && Count == 0);
Debug.Assert(count >= 3);
if (count < 3)
{
return;
}
for (var i = 1; i < count; ++i)
{
var v1 = vertices[i - 1];
var v2 = vertices[i];
// If the code crashes here, it means your vertices are too close together.
Debug.Assert(Vector2.DistanceSquared(v1, v2) > Settings.LinearSlop * Settings.LinearSlop);
}
Count = count + 1;
Vertices = new Vector2[Count];
Array.Copy(vertices, Vertices, count);
Vertices[count] = Vertices[0];
PrevVertex = Vertices[Count - 2];
NextVertex = Vertices[1];
}
/// Initialize the proxy using a vertex cloud and radius. The vertices
/// must remain in scope while the proxy is in use.
public void Set(Vector2[] vertices, int count, float radius)
{
Vertices = new Vector2[vertices.Length];
Array.Copy(vertices, Vertices, vertices.Length);
Count = count;
Radius = radius;
}
/// Initialize the bodies, anchors, and reference angle using a world
/// anchor point.
public void Initialize(Body bA, Body bB, Vector2 anchor)
{
BodyA = bA;
BodyB = bB;
LocalAnchorA = BodyA.GetLocalPoint(anchor);
LocalAnchorB = BodyB.GetLocalPoint(anchor);
ReferenceAngle = BodyB.GetAngle() - BodyA.GetAngle();
}
/// Initialize the proxy using the given shape. The shape
/// must remain in scope while the proxy is in use.
internal void Set(Shape shape, int index)
{
switch (shape)
{
case CircleShape circle:
{
Vertices = new[] { circle.Position };
Count = 1;
Radius = circle.Radius;
}
break;
case PolygonShape polygon:
{
Vertices = polygon.Vertices;
Count = polygon.Count;
Radius = polygon.Radius;
}
break;
case ChainShape chain:
{
Debug.Assert(0 <= index && index < chain.Count);
Count = 2;
Vertices = new Vector2[Count];
Vertices[0] = chain.Vertices[index];
if (index + 1 < chain.Count)
{
Vertices[1] = chain.Vertices[index + 1];
}
else
{
Vertices[1] = chain.Vertices[0];
}
Radius = chain.Radius;
}
break;
case EdgeShape edge:
{
Vertices = new[]
{
edge.Vertex1,
edge.Vertex2
};
Count = 2;
Radius = edge.Radius;
}
break;
default:
throw new NotSupportedException();
}
}
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;
}
/// Initialize the bodies and offsets using the current transforms.
public void Initialize(Body bA, Body bB)
{
BodyA = bA;
BodyB = bB;
var xB = BodyB.GetPosition();
LinearOffset = BodyA.GetLocalPoint(xB);
var angleA = BodyA.GetAngle();
var angleB = BodyB.GetAngle();
AngularOffset = angleB - angleA;
}
internal MouseJoint(MouseJointDef def)
: base(def)
{
Target = def.Target;
_localAnchorB = MathUtils.MulT(BodyB.GetTransform(), Target);
MaxForce = def.MaxForce;
Stiffness = def.Stiffness;
Damping = def.Damping;
_impulse.SetZero();
_beta = 0.0f;
_gamma = 0.0f;
}
/// Get the current joint translation, usually in meters.
public float GetJointTranslation()
{
var bA = BodyA;
var bB = BodyB;
var pA = bA.GetWorldPoint(_localAnchorA);
var pB = bB.GetWorldPoint(_localAnchorB);
var d = pB - pA;
var axis = bA.GetWorldVector(_localXAxisA);
var translation = Vector2.Dot(d, axis);
return(translation);
}
public PulleyJoint(PulleyJointDef def) : base(def)
{
_groundAnchorA = def.GroundAnchorA;
_groundAnchorB = def.GroundAnchorB;
_localAnchorA = def.LocalAnchorA;
_localAnchorB = def.LocalAnchorB;
_lengthA = def.LengthA;
_lengthB = def.LengthB;
Debug.Assert(!def.Ratio.Equals(0.0f));
_ratio = def.Ratio;
_constant = def.LengthA + _ratio * def.LengthB;
_impulse = 0.0f;
}
internal DistanceJoint(DistanceJointDef def)
: base(def)
{
_localAnchorA = def.LocalAnchorA;
_localAnchorB = def.LocalAnchorB;
_length = Math.Max(def.Length, Settings.LinearSlop);
_minLength = Math.Max(def.MinLength, Settings.LinearSlop);
_maxLength = Math.Max(def.MaxLength, _minLength);
Stiffness = def.Stiffness;
Damping = def.Damping;
_impulse = 0.0f;
_gamma = 0.0f;
_bias = 0.0f;
_impulse = 0.0f;
_lowerImpulse = 0.0f;
_upperImpulse = 0.0f;
_currentLength = 0.0f;
}
/// <summary>
/// Create a chain with ghost vertices to connect multiple chains together.
/// </summary>
/// <param name="vertices">an array of vertices, these are copied</param>
/// <param name="count">the vertex count</param>
/// <param name="prevVertex">previous vertex from chain that connects to the start</param>
/// <param name="nextVertex">next vertex from chain that connects to the end</param>
public void CreateChain(Vector2[] vertices, int count, Vector2 prevVertex, Vector2 nextVertex)
{
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(
Vector2.DistanceSquared(vertices[i - 1], vertices[i])
> Settings.LinearSlop * Settings.LinearSlop);
}
Count = count;
Vertices = new Vector2[count];
Array.Copy(vertices, Vertices, count);
PrevVertex = prevVertex;
NextVertex = nextVertex;
}
internal RevoluteJoint(RevoluteJointDef def)
: base(def)
{
LocalAnchorA = def.LocalAnchorA;
LocalAnchorB = def.LocalAnchorB;
ReferenceAngle = def.ReferenceAngle;
_impulse.SetZero();
_motorImpulse = 0.0f;
_axialMass = 0.0f;
_lowerImpulse = 0.0f;
_upperImpulse = 0.0f;
_lowerAngle = def.LowerAngle;
_upperAngle = def.UpperAngle;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_angle = 0.0f;
}
/// Get the current joint linear speed, usually in meters per second.
public float GetJointLinearSpeed()
{
var bA = BodyA;
var bB = BodyB;
var rA = MathUtils.Mul(bA.Transform.Rotation, _localAnchorA - bA.Sweep.LocalCenter);
var rB = MathUtils.Mul(bB.Transform.Rotation, _localAnchorB - bB.Sweep.LocalCenter);
var p1 = bA.Sweep.C + rA;
var p2 = bB.Sweep.C + rB;
var d = p2 - p1;
var axis = MathUtils.Mul(bA.Transform.Rotation, _localXAxisA);
var vA = bA.LinearVelocity;
var vB = bB.LinearVelocity;
var wA = bA.AngularVelocity;
var wB = bB.AngularVelocity;
var speed = Vector2.Dot(d, MathUtils.Cross(wA, axis))
+ Vector2.Dot(axis, vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA));
return(speed);
}
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);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = _joint1.BodyA;
BodyA = _joint1.BodyB;
// Body B on joint1 must be dynamic
Debug.Assert(BodyA.BodyType == BodyType.DynamicBody);
// 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 = Vector2.Dot(pA - pC, _localAxisC);
}
_bodyD = _joint2.BodyA;
BodyB = _joint2.BodyB;
// Body B on joint2 must be dynamic
Debug.Assert(BodyB.BodyType == BodyType.DynamicBody);
// 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 = Vector2.Dot(pB - pD, _localAxisD);
}
_ratio = def.Ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
/// Create a convex hull from the given array of local points.
/// The count must be in the range [3, b2_maxPolygonVertices].
/// @warning the points may be re-ordered, even if they form a convex polygon
/// @warning collinear points are handled but not removed. Collinear points
/// may lead to poor stacking behavior.
public void Set(Vector2[] vertices, int count = -1)
{
if (count == -1)
{
count = vertices.Length;
}
Debug.Assert(3 <= count && count <= MaxPolygonVertices);
// 顶点数小于3,视为盒子
if (count < 3)
{
SetAsBox(1.0f, 1.0f);
return;
}
// 顶点数最大为 MaxPolygonVertices
var n = Math.Min(count, MaxPolygonVertices);
// Perform welding and copy vertices into local buffer.
Span <Vector2> ps = stackalloc Vector2[MaxPolygonVertices];
var tempCount = 0;
for (var i = 0; i < n; ++i)
{
var v = vertices[i];
var unique = true;
for (var j = 0; j < tempCount; ++j)
{
if (Vector2.DistanceSquared(v, ps[j])
< 0.5f * Settings.LinearSlop * (0.5f * Settings.LinearSlop))
{
unique = false;
break;
}
}
if (unique)
{
ps[tempCount] = v;
tempCount++;
}
}
n = tempCount;
if (n < 3)
{
// Polygon is degenerate.
throw new InvalidOperationException("Invalid polygon shape");
}
// Create the convex hull using the Gift wrapping algorithm
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
// Find the right most point on the hull
var i0 = 0;
var x0 = ps[0].x;
for (var i = 1; i < n; ++i)
{
var x = ps[i].x;
if (x > x0 || x.Equals(x0) && ps[i].y < ps[i0].y)
{
i0 = i;
x0 = x;
}
}
Span <int> hull = stackalloc int[MaxPolygonVertices];
var m = 0;
var ih = i0;
for (;;)
{
Debug.Assert(m < MaxPolygonVertices);
hull[m] = ih;
var ie = 0;
for (var j = 1; j < n; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
var r = ps[ie] - ps[hull[m]];
var v = ps[j] - ps[hull[m]];
var c = MathUtils.Cross(r, v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c.Equals(0.0f) && v.LengthSquared() > r.LengthSquared())
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
if (m < 3)
{
// Polygon is degenerate.
throw new InvalidOperationException("Invalid polygon shape");
}
Count = m;
// Copy vertices.
for (var i = 0; i < m; ++i)
{
Vertices[i] = ps[hull[i]];
}
// Compute normals. Ensure the edges have non-zero length.
for (var i = 0; i < m; ++i)
{
var i1 = i;
var i2 = i + 1 < m ? i + 1 : 0;
var edge = Vertices[i2] - Vertices[i1];
Debug.Assert(edge.LengthSquared() > Settings.Epsilon * Settings.Epsilon);
Normals[i] = MathUtils.Cross(edge, 1.0f);
Normals[i].Normalize();
}
// Compute the polygon centroid.
Centroid = ComputeCentroid(Vertices, m);
}
public static Vector2 ToVec2(DeusaldSharp.Vector2 vector2)
{
return(new Vector2(vector2.x, vector2.y));
}
/// <inheritdoc />
public override Vector2 GetReactionForce(float inv_dt)
{
var P = new Vector2(_impulse.x, _impulse.y);
return(inv_dt * P);
}
