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); }