public static void FatArrow(Vector3 pos, Vector3 direction, float width = 1f)
{
Vector3 arrowWidth = direction.normalized * width;
Vector3 arrowLength = direction - arrowWidth;
Handles.DrawLine(pos, pos + arrowLength);
Vector3 trunkDirection = Vector2Helpers.Rotate(arrowWidth, 90);
Handles.DrawLine(pos, pos + trunkDirection);
Handles.DrawLine(pos + trunkDirection, pos + trunkDirection + arrowLength);
Vector3 arrowBottomWing = Vector2Helpers.Rotate(arrowWidth / 2, -90);
Handles.DrawLine(pos + arrowLength, pos + arrowLength + arrowBottomWing);
Vector3 arrowTopWing = trunkDirection / 2;
Handles.DrawLine(pos + trunkDirection + arrowLength, pos + trunkDirection + arrowLength + arrowTopWing);
Vector3 arrowEnd = pos + (trunkDirection / 2) + arrowLength + arrowWidth;
Handles.DrawLine(pos + arrowLength + arrowBottomWing, arrowEnd);
Handles.DrawLine(pos + trunkDirection + arrowLength + arrowTopWing, arrowEnd);
}
private Vector2 GenerateRelictVelocity()
{
Vector2 normal = Vector2Helpers.DegreeToVector2(RandomHelpers.Range(relicAngle));
float velocity = TileHelpers.TileToWorld(RandomHelpers.Range(relicTileVelocity));
return(normal * velocity);
}
public static void Arrow(Vector3 pos, Vector3 direction, float width = 1f)
{
Vector3 arrowTip = pos + direction;
Handles.DrawLine(pos, arrowTip);
Handles.DrawLine(arrowTip, arrowTip + (Vector3)Vector2Helpers.Rotate(-direction.normalized * width, 45));
Handles.DrawLine(arrowTip, arrowTip + (Vector3)Vector2Helpers.Rotate(-direction.normalized * width, -45));
}
internal bool HasWallInFront()
{
Vector2 extents = boxCollider.bounds.extents;
Vector2 origin = boxCollider.bounds.center;
Vector2 rayOrigin = origin;
float skingWidth = Constants.SKIN_WIDTH;
int axis = rotation.IsHorizontal() ? 0 : 1;
float sign = transform.right[axis];
rayOrigin += Vector2Helpers.AxisVector(axis, sign * (extents[axis] - skingWidth));
Vector2 rayVector = rotation.GetFrontVector();
RaycastHit2D hit = CastRay(rayOrigin, rayVector, frontRayLength + skingWidth, frontRayLayerMask);
return(hit == true);
}
internal override void InitVelocityConstraints(SolverData data)
{
_indexA = BodyA.IslandIndex[data.IslandIndex];
_indexB = BodyB.IslandIndex[data.IslandIndex];
_localCenterA = BodyA.LocalCenter;
_localCenterB = BodyB.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIA = BodyA.InvI;
_invIB = BodyB.InvI;
float aA = data.Angles[_indexA];
Vector2 vA = data.LinearVelocities[_indexA];
float wA = data.AngularVelocities[_indexA];
float aB = data.Angles[_indexB];
Vector2 vB = data.LinearVelocities[_indexB];
float wB = data.AngularVelocities[_indexB];
Quaternion2D qA = new(aA), qB = new(aB);
// Compute the effective mass matrix.
_rA = Transform.Mul(qA, LocalAnchorA - _localCenterA);
_rB = Transform.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Span <Vector2> K = stackalloc Vector2[2];
K[0].X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
K[0].Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
K[1].X = K[0].Y;
K[1].Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;
_linearMass = Vector2Helpers.Inverse(K);
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
// ReSharper disable once ConditionIsAlwaysTrueOrFalse
if (IoCManager.Resolve <IConfigurationManager>().GetCVar(CVars.WarmStarting))
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.DtRatio;
_angularImpulse *= data.DtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
vA -= P * mA;
wA -= iA * (Vector2.Cross(_rA, P) + _angularImpulse);
vB += P * mB;
wB += iB * (Vector2.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
data.LinearVelocities[_indexA] = vA;
data.AngularVelocities[_indexA] = wA;
data.LinearVelocities[_indexB] = vB;
data.AngularVelocities[_indexB] = wB;
}
