/// <summary>Gets the world linear velocity of a world point attached to this body.</summary>
/// <param name="worldPoint">The point in world coordinates.</param>
/// <returns>The world velocity of a point.</returns>
public Vector2 GetLinearVelocityFromWorldPoint(WorldPoint worldPoint)
{
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(LinearVelocityInternal +
Vector2Util.Cross(AngularVelocityInternal, (Vector2)(worldPoint - Sweep.CenterOfMass)));
// ReSharper restore RedundantCast
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
var vpA = vA + Vector2Util.Cross(wA, ref _tmp.RotA);
var vpB = vB + Vector2Util.Cross(wB, ref _tmp.RotB);
var cdot = -Vector2Util.Dot(ref _tmp.AxisA, ref vpA) - _ratio * Vector2Util.Dot(ref _tmp.AxisB, ref vpB);
var impulse = -_tmp.Mass * cdot;
_impulse += impulse;
var pA = -impulse * _tmp.AxisA;
var pB = -_ratio * impulse * _tmp.AxisB;
vA += _tmp.InverseMassA * pA;
wA += _tmp.InverseInertiaA * Vector2Util.Cross(ref _tmp.RotA, ref pA);
vB += _tmp.InverseMassB * pB;
wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref pB);
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
// Cdot = v + cross(w, r)
var cdot = vB + Vector2Util.Cross(wB, ref _tmp.RotB);
var impulse = _tmp.Mass * -(cdot + _tmp.C + _tmp.Gamma * _impulse);
var oldImpulse = _impulse;
_impulse += impulse;
var maxImpulse = step.DeltaT * _maxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
vB += _tmp.InverseMassB * impulse;
wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref impulse);
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
/// <summary>Initializes this joint with the specified parameters.</summary>
internal void Initialize(
WorldPoint anchor,
Vector2 axis,
float frequency,
float dampingRatio,
float maxMotorTorque,
float motorSpeed,
bool enableMotor)
{
_localAnchorA = BodyA.GetLocalPoint(anchor);
_localAnchorB = BodyB.GetLocalPoint(anchor);
_localXAxisA = BodyA.GetLocalVector(axis);
_localYAxisA = Vector2Util.Cross(1.0f, ref _localXAxisA);
_frequency = frequency;
_dampingRatio = dampingRatio;
_maxMotorTorque = maxMotorTorque;
_motorSpeed = motorSpeed;
_enableMotor = enableMotor;
_impulse = 0;
_motorImpulse = 0;
_springImpulse = 0;
}
public static float GetDistance(StraightLine self, Vector2 p)
{
var v = p - self.p;
// 本当は this.Cross(this.dir, v) / this.dir.magnitude だが、DirectionのMagnitudeは1.
return(Mathf.Abs(Vector2Util.Cross(self.dir, v)));
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
var mA = _tmp.InverseMassA;
var mB = _tmp.InverseMassB;
var iA = _tmp.InverseInertiaA;
var iB = _tmp.InverseInertiaB;
var h = step.DeltaT;
var invH = step.InverseDeltaT;
// Solve angular friction
{
var cdot = wB - wA + invH * _correctionFactor * _tmp.AngularError;
var impulse = -_tmp.AngularMass * cdot;
var oldImpulse = _angularImpulse;
var maxImpulse = h * _maxTorque;
_angularImpulse = MathHelper.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
var cdot = vB + Vector2Util.Cross(wB, ref _tmp.RotB) - vA - Vector2Util.Cross(wA, ref _tmp.RotA) +
invH * _correctionFactor * _tmp.LinearError;
var impulse = -(_tmp.LinearMass * cdot);
var oldImpulse = _linearImpulse;
_linearImpulse += impulse;
var maxImpulse = h * _maxForce;
if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * Vector2Util.Cross(ref _tmp.RotA, ref impulse);
vB += mB * impulse;
wB += iB * Vector2Util.Cross(ref _tmp.RotB, ref impulse);
}
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
public static bool IsCrossing(HalfStraightLine self, HalfStraightLine other)
{
return(
Vector2Util.Cross(self.dir, other.p - self.p) * Vector2Util.Cross(self.dir, other.dir) < 0f &&
GetIntersectionParametric(self.p, self.dir, other.p, other.dir) >= 0f
);
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
// cDot = dot(u, v + cross(w, r))
var vpA = vA + Vector2Util.Cross(wA, ref _tmp.RotA);
var vpB = vB + Vector2Util.Cross(wB, ref _tmp.RotB);
var cDot = Vector2Util.Dot(_tmp.U, vpB - vpA);
var impulse = -_tmp.Mass * (cDot + _tmp.Bias + _tmp.Gamma * _impulse);
_impulse += impulse;
var p = impulse * _tmp.U;
vA -= _tmp.InverseMassA * p;
wA -= _tmp.InverseInertiaA * Vector2Util.Cross(ref _tmp.RotA, ref p);
vB += _tmp.InverseMassB * p;
wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref p);
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
/// <summary>Computes the centroid of the vertices, used in initialization.</summary>
/// <returns>The centroid.</returns>
private LocalPoint ComputeCentroid()
{
var c = LocalPoint.Zero;
var area = 0.0f;
const float inv3 = 1.0f / 3.0f;
for (var i = 0; i < Count; ++i)
{
// Triangle vertices.
var p1 = LocalPoint.Zero;
var p2 = Vertices[i];
var p3 = i + 1 < Count ? Vertices[i + 1] : Vertices[0];
var e1 = p2 - p1;
var e2 = p3 - p1;
var triangleArea = 0.5f * Vector2Util.Cross(ref e1, ref e2);
area += triangleArea;
// Area weighted centroid
c += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
System.Diagnostics.Debug.Assert(area > float.Epsilon);
c *= 1.0f / area;
return(c);
}
/// <summary>Initializes this joint with the specified parameters.</summary>
internal void Initialize(
WorldPoint anchor,
Vector2 axis,
float lowerTranslation = 0,
float upperTranslation = 0,
float maxMotorForce = 0,
float motorSpeed = 0,
bool enableLimit = false,
bool enableMotor = false)
{
_localAnchorA = BodyA.GetLocalPoint(anchor);
_localAnchorB = BodyB.GetLocalPoint(anchor);
_localXAxisA = BodyA.GetLocalVector(axis);
_localXAxisA.Normalize();
_localYAxisA = Vector2Util.Cross(1.0f, ref _localXAxisA);
_referenceAngle = BodyB.Angle - BodyA.Angle;
_lowerTranslation = lowerTranslation;
_upperTranslation = upperTranslation;
_maxMotorForce = maxMotorForce;
_motorSpeed = motorSpeed;
_enableLimit = enableLimit;
_enableMotor = enableMotor;
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
_limitState = LimitState.Inactive;
}
/// <summary>
/// 方块旋转,踢墙和踢地板
/// </summary>
public void Rotation()
{
if (dropStat != DropStat.Dropping)
{
return;
}
Vector2Int[] newpos = new Vector2Int[4];
// 计算原地旋转后的坐标
for (int i = 0; i < blocks.Length; i++)
{
newpos[i] = Vector2Util.RotateClockWise(blocks[i].Coord, pivot.localPosition);
}
//踢墙检测
for (int j = 0; j < TickOffsetPoint.Length; j++)
{
Vector2Int[] kickpos = new Vector2Int[4];
for (int i = 0; i < blocks.Length; i++)
{
kickpos[i] = newpos[i] + Vector2Int.up;
}
if (CanAction(newpos)) // 能转
{
nextDroptime = 0f;
for (int i = 0; i < blocks.Length; i++)
{
blocks[i].Coord = newpos[i];
}
return;
}
}
}
public void MoveTowardsByAngle()
{
Assert.AreEqual(Vector2Util.FromDeg(45f), Vector2Util.MoveTowardsByAngle(new Vector2(0f, 0f), new Vector2(-1f, 0.1f), 45f));
Assert.AreEqual(Vector2Util.FromDeg(90f), Vector2Util.MoveTowardsByAngle(new Vector2(0f, 0f), new Vector2(-1f, 0.1f), 90f));
Assert.AreEqual(Vector2Util.FromDeg(-45f), Vector2Util.MoveTowardsByAngle(new Vector2(0f, 0f), new Vector2(-1f, -0.1f), 45f));
Assert.AreEqual(Vector2Util.FromDeg(-90f), Vector2Util.MoveTowardsByAngle(new Vector2(0f, 0f), new Vector2(-1f, -0.1f), 90f));
}
/// <summary>Solves the velocity constraints.</summary>
/// <param name="step">The time step for this update.</param>
/// <param name="velocities">The velocities of the related bodies.</param>
internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
{
var vA = velocities[_tmp.IndexA].LinearVelocity;
var wA = velocities[_tmp.IndexA].AngularVelocity;
var vB = velocities[_tmp.IndexB].LinearVelocity;
var wB = velocities[_tmp.IndexB].AngularVelocity;
var mA = _tmp.InverseMassA;
var mB = _tmp.InverseMassB;
var iA = _tmp.InverseInertiaA;
var iB = _tmp.InverseInertiaB;
if (_frequency > 0.0f)
{
var cdot2 = wB - wA;
var impulse2 = -_tmp.Mass.Column3.Z * (cdot2 + _tmp.Bias + _tmp.Gamma * _impulse.Z);
_impulse.Z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
var cdot1 = vB + Vector2Util.Cross(wB, ref _tmp.RotB) - vA - Vector2Util.Cross(wA, ref _tmp.RotA);
var impulse1 = -(_tmp.Mass * cdot1);
_impulse.X += impulse1.X;
_impulse.Y += impulse1.Y;
var p = impulse1;
vA -= mA * p;
wA -= iA * Vector2Util.Cross(ref _tmp.RotA, ref p);
vB += mB * p;
wB += iB * Vector2Util.Cross(ref _tmp.RotB, ref p);
}
else
{
var cdot1 = vB + Vector2Util.Cross(wB, ref _tmp.RotB) - vA - Vector2Util.Cross(wA, ref _tmp.RotA);
var cdot2 = wB - wA;
var cdot = new Vector3(cdot1.X, cdot1.Y, cdot2);
var impulse = -(_tmp.Mass * cdot);
_impulse += impulse;
var p = new Vector2(impulse.X, impulse.Y);
vA -= mA * p;
wA -= iA * (Vector2Util.Cross(ref _tmp.RotA, ref p) + impulse.Z);
vB += mB * p;
wB += iB * (Vector2Util.Cross(ref _tmp.RotB, ref p) + impulse.Z);
}
velocities[_tmp.IndexA].LinearVelocity = vA;
velocities[_tmp.IndexA].AngularVelocity = wA;
velocities[_tmp.IndexB].LinearVelocity = vB;
velocities[_tmp.IndexB].AngularVelocity = wB;
}
private static Vector2 GetIntersectionWithLineSegment(LineSegment self, Vector2 p, Vector2 dir)
{
var d1 = Mathf.Abs(Vector2Util.Cross(dir, self.p0 - p));
var d2 = Mathf.Abs(Vector2Util.Cross(dir, self.p1 - p));
var t = d1 / (d1 + d2);
return(self.p0 + t * self.dir);
}
/// <summary>
/// Get the mass data for this fixture. The mass data is based on the density and the shape. The rotational
/// inertia is about the shape's origin. This operation may be expensive.
/// </summary>
/// <param name="mass">The mass of this fixture.</param>
/// <param name="center">The center of mass relative to the local origin.</param>
/// <param name="inertia">The inertia about the local origin.</param>
internal override void GetMassData(out float mass, out LocalPoint center, out float inertia)
{
mass = Density * MathHelper.Pi * Radius * Radius;
center = Center;
// Inertia about the local origin.
inertia = mass * (0.5f * Radius * Radius + Vector2Util.Dot(ref Center, ref Center));
}
public static Quaternion LookAtAngle(Chara self, Vector3 position, float maxDelta)
{
Vector2 vec = self.data.rotation * Vector3.forward;
Vector2 toTargetVec = position - self.data.position;
var angleSpeed = maxDelta * Stage.Current.time.dt;
Vector2 vec2 = Vector2Util.MoveTowardsByAngle(vec, toTargetVec, angleSpeed);
return(Quaternion.LookRotation(vec2));
}
public static bool IsCrossing(LineSegment self, LineSegment other)
{
return((
Vector2Util.Cross(self.dir, other.p0 - self.p0) *
Vector2Util.Cross(self.dir, other.p1 - self.p0) < EPS
) && (
Vector2Util.Cross(other.dir, self.p0 - other.p0) *
Vector2Util.Cross(other.dir, self.p1 - other.p0) < EPS
));
}
/// <summary>This returns true if the position errors are within tolerance, allowing an early exit from the iteration loop.</summary>
/// <param name="positions">The positions of the related bodies.</param>
/// <returns>
/// <c>true</c> if the position errors are within tolerance.
/// </returns>
internal override bool SolvePositionConstraints(Position[] positions)
{
var cA = positions[_tmp.IndexA].Point;
var aA = positions[_tmp.IndexA].Angle;
var cB = positions[_tmp.IndexB].Point;
var aB = positions[_tmp.IndexB].Angle;
var qA = new Rotation(aA);
var qB = new Rotation(aB);
var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var d = (Vector2)(cB - cA) + (rB - rA);
// ReSharper restore RedundantCast
var ay = qA * _localYAxisA;
var sAy = Vector2Util.Cross(d + rA, ay);
var sBy = Vector2Util.Cross(rB, ay);
var c = Vector2Util.Dot(ref d, ref ay);
var k = _tmp.InverseMassA + _tmp.InverseMassB + _tmp.InverseInertiaA * _tmp.SAy * _tmp.SAy +
_tmp.InverseInertiaB * _tmp.SBy * _tmp.SBy;
float impulse;
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
if (k != 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
{
impulse = -c / k;
}
else
{
impulse = 0.0f;
}
var p = impulse * ay;
var lA = impulse * sAy;
var lB = impulse * sBy;
cA -= _tmp.InverseMassA * p;
aA -= _tmp.InverseInertiaA * lA;
cB += _tmp.InverseMassB * p;
aB += _tmp.InverseInertiaB * lB;
positions[_tmp.IndexA].Point = cA;
positions[_tmp.IndexA].Angle = aA;
positions[_tmp.IndexB].Point = cB;
positions[_tmp.IndexB].Angle = aB;
return(System.Math.Abs(c) <= Settings.LinearSlop);
}
//额外辅助方法部分,暂定到达目标就算停止
public bool IsAtDestination()
{
//原作中crowd用2D做检查，simple用3D做检查。
//这目前来看似乎没什么道理，所以索性整合在一起做一个方法，还可以返回给上层UnityAgent用
bool Check3D = Vector3Util.SloppyEquals(desiredPosition.point, plannerGoal.point, 0.005f);
Vector3 pos = desiredPosition.point;
Vector3 goal = plannerGoal.point;
bool Check2D = Vector2Util.SloppyEquals(new Vector2(pos.x, pos.z), new Vector2(goal.x, goal.z), 0.005f);
return(Check3D | Check2D);
}
public static bool IsCrossing(HalfStraightLine self, LineSegment other)
{
var v1 = other.p0 - self.p;
var v2 = other.p1 - self.p;
return(
(Vector2.Dot(self.dir, v1) >= 0f || Vector2.Dot(self.dir, v2) >= 0f) &&
Vector2Util.Cross(self.dir, v1) * Vector2Util.Cross(self.dir, v2) < 0f &&
(GetIntersectionParametric(self.p, self.dir, other.p0, other.dir) >= 0f)
);
}
public bool MapEmpty(Vector2Int direction)
{
foreach (var collisionArea in directionCollisionCheckMap[direction])
{
if (mapController.map.GetMapUnit(Vector2Util.RoundToInt(LocalToMap(collisionArea))) != null)
{
return(false);
}
}
return(true);
}
/// Clipping for contact manifolds.
private static int ClipSegmentToLine(
out FixedArray2 <ClipVertex> vOut,
FixedArray2 <ClipVertex> vIn,
Vector2 normal,
float offset,
int vertexIndexA)
{
// Satisfy outs.
vOut = new FixedArray2 <ClipVertex>();
// Start with no output points
var numOut = 0;
// Calculate the distance of end points to the line
var distance0 = Vector2Util.Dot(ref normal, ref vIn.Item1.Vertex) - offset;
var distance1 = Vector2Util.Dot(ref normal, ref vIn.Item2.Vertex) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
{
vOut.Item1 = vIn.Item1;
++numOut;
if (distance1 <= 0.0f)
{
vOut.Item2 = vIn.Item2;
++numOut;
}
}
else if (distance1 <= 0.0f)
{
vOut.Item1 = vIn.Item2;
++numOut;
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
System.Diagnostics.Debug.Assert(numOut == 1);
// Find intersection point of edge and plane
var interp = distance0 / (distance0 - distance1);
vOut.Item2.Vertex = vIn.Item1.Vertex + interp * (vIn.Item2.Vertex - vIn.Item1.Vertex);
// VertexA is hitting edgeB.
vOut.Item2.Id.Feature.IndexA = (byte)vertexIndexA;
vOut.Item2.Id.Feature.IndexB = vIn.Item1.Id.Feature.IndexB;
vOut.Item2.Id.Feature.TypeA = (byte)ContactFeature.FeatureType.Vertex;
vOut.Item2.Id.Feature.TypeB = (byte)ContactFeature.FeatureType.Face;
++numOut;
}
return(numOut);
}
/// <summary>Test the specified point for containment in this fixture.</summary>
/// <param name="localPoint">The point in local coordinates.</param>
/// <returns>Whether the point is contained in this fixture or not.</returns>
public override bool TestPoint(Vector2 localPoint)
{
for (var i = 0; i < Count; ++i)
{
if (Vector2Util.Dot(Normals[i], localPoint - Vertices[i]) > 0.0f)
{
return(false);
}
}
return(true);
}
public bool collide(Vector2 point)
{
if (Vector2Util.orMin(point, Min) == true)
{
return(false);
}
if (Vector2Util.orMax(point, Min) == true)
{
return(false);
}
return(true);
}
public bool collide(AABB2D r)
{
if (Vector2Util.orMin(Max, r.Min) == true)
{
return(false);
}
if (Vector2Util.orMin(r.Max, Min) == true)
{
return(false);
}
return(true);
}
public static float GetDistance(LineSegment self, Vector2 p)
{
if (Vector2.Dot(self.dir, p - self.p0) < EPS)
{
return(Vector2.Distance(self.p0, p));
}
if (Vector2.Dot(-self.dir, p - self.p1) < EPS)
{
return(Vector2.Distance(self.p1, p));
}
return(Mathf.Abs(Vector2Util.Cross(self.dir, p - self.p0)) / self.dir.magnitude);
}
public float Evaluate(int indexA, int indexB, float t)
{
WorldTransform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case Type.Points:
{
var localPointA = _proxyA.Vertices[indexA];
var localPointB = _proxyB.Vertices[indexB];
var pointA = xfA.ToGlobal(localPointA);
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointB - pointA), _axis));
// ReSharper restore RedundantCast
}
case Type.FaceA:
{
var normal = xfA.Rotation * _axis;
var pointA = xfA.ToGlobal(_localPoint);
var localPointB = _proxyB.Vertices[indexB];
var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointB - pointA), normal));
// ReSharper restore RedundantCast
}
case Type.FaceB:
{
var normal = xfB.Rotation * _axis;
var pointB = xfB.ToGlobal(_localPoint);
var localPointA = _proxyA.Vertices[indexA];
var pointA = xfA.ToGlobal(localPointA);
// ReSharper disable RedundantCast Necessary for FarPhysics.
return(Vector2Util.Dot((Vector2)(pointA - pointB), normal));
// ReSharper restore RedundantCast
}
default:
throw new ArgumentOutOfRangeException();
}
}
/// <summary>Sets the mass properties for this body, overriding properties from any fixtures attached to this body.</summary>
/// <param name="mass">The overall mass of the body.</param>
/// <param name="center">The center of mass, relative to the local origin.</param>
/// <param name="inertia">The rotational inertia about the local origin.</param>
public void SetMassData(float mass, Vector2 center, float inertia)
{
// Do not allow changing a body's type during an update.
if (Simulation.IsLocked)
{
throw new System.InvalidOperationException("Cannot change mass data during update.");
}
// Ignore if we're not a dynamic body.
if (TypeInternal != BodyType.Dynamic)
{
return;
}
// Make sure we have a positive mass.
MassInternal = mass;
if (MassInternal <= 0)
{
MassInternal = 1;
InverseMass = 1;
}
else
{
InverseMass = 1 / MassInternal;
}
// Make sure we have a positive inertia.
if (inertia <= 0)
{
_inertia = 0;
InverseInertia = 0;
}
else
{
_inertia = inertia - MassInternal * center.LengthSquared();
InverseInertia = 1 / _inertia;
}
// Move center of mass.
var oldCenter = Sweep.CenterOfMass;
Sweep.LocalCenter = center;
Sweep.CenterOfMass0 = Sweep.CenterOfMass = Transform.ToGlobal(Sweep.LocalCenter);
// Update center of mass velocity.
// ReSharper disable RedundantCast Necessary for FarPhysics.
LinearVelocityInternal += Vector2Util.Cross(
AngularVelocityInternal, (Vector2)(Sweep.CenterOfMass - oldCenter));
// ReSharper restore RedundantCast
}
public TurtleMoveBack(ZLogoActionBase turtleAction, float distance)
: base(turtleAction)
{
//_startTurleInfo = turtleAction.GetEndTurleInfo().Clone();
var angle = _startTurleInfo.Angle + 180;
var speed = _startTurleInfo.MoveSpeed;
Vector2 ToPosition = Vector2Util.GetPointByPolar(_startTurleInfo.X, _startTurleInfo.Y, distance, angle);
//_endTurleInfo = _startTurleInfo.Clone();
_endTurleInfo.X = ToPosition.X;
_endTurleInfo.Y = ToPosition.Y;
speedX = (float)(speed * MathUtil.Cos(angle));
speedY = (float)(speed * MathUtil.Sin(angle));
}
public void Update()
{
if (mapController != null)
{
mapPosition = mapController.map.WorldToMapPoint(Position);
if (clamped)
{
mapPosition = mapController.map.ClampPosition(mapPosition);
}
if (rounded)
{
mapPosition = Vector2Util.RoundToInt(mapPosition);
}
Position = mapController.map.MapToWorldPoint(mapPosition);
}
}
