public void DotProduct_SameVector_ReturnsMSquared()
{
Vector2 test = new Vector2(2.0, 0.0);
double result = test.Dot(test);
Assert.AreEqual(4.0, result, Epsilon);
}
/// <summary>
/// Checks if a point is inside a cone.
/// </summary>
public static bool IsPointInsideCone(Vector2 origin, Vector2 direction, Vector2 point, double radianAngle, int maxDistance)
{
Vector2 distanceVector = new Vector2(point.X - origin.X, point.Y - origin.Y);
double length = distanceVector.Normalize(); //Returns not the normalized distance vector, but the length of the vector. A shortcut.
if (length > maxDistance)
{
return false;
}
return (direction.Dot(distanceVector) >= Math.Cos(radianAngle));
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float L1 = df.X * _s1 + df.Y + df.Z * _a1;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float L1 = df.X * _s1 + df.Y;
float L2 = df.X * _s2 + df.Y;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
public static float Project(Vector2 vector, Vector2 axis) => Vector2.Dot(vector, axis / axis.Length());
public void Solve(TimeStep step, Vector2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Integrate velocities.
b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.Dt * b._invI * b._torque;
// Reset forces.
b._force = Vector2.Zero;
b._torque = 0.0f;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
b._linearVelocity *= Box2DX.Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= Box2DX.Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
}
ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);
// Initialize velocity constraints.
contactSolver.InitVelocityConstraints(step);
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(step);
}
// Solve velocity constraints.
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
contactSolver.FinalizeVelocityConstraints();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.IsStatic())
{
continue;
}
// Check for large velocities.
Vector2 translation = step.Dt * b._linearVelocity;
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation.Normalized();
}
float rotation = step.Dt * b._angularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
}
else
{
b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
}
}
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
// Integrate
b._sweep.C += step.Dt * b._linearVelocity;
b._sweep.A += step.Dt * b._angularVelocity;
// Compute new Transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b._invMass == 0.0f)
{
continue;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
b._angularVelocity *b._angularVelocity > angTolSqr ||
Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.Dt;
minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
}
}
public override void SetMovementInput(Vector2 movementInput)
{
base.SetMovementInput(movementInput);
if (movementInput.magnitude < walkingJoystickMaxTilt)//} && rigidbody.velocity.magnitude <= maxWalkingVelocity + 1){
{
isRunning = false;
}
else
{
isRunning = true;
}
movementInput.Normalize();
movement.Set(movementInput.x, 0, movementInput.y);
movement = Camera.main.transform.TransformDirection(movement);
movement.y = 0f;
movement.Normalize();
movementInputFromCameraPOV.Set(movement.x, movement.z);
currentVelocityNormalized.Set(rigidbody.velocity.x, rigidbody.velocity.z);
currentVelocityNormalized.Normalize();
//Debug.Log("last: " + currentVelocityNormalized + " , current: " + movementInputFromCameraPOV);
if (Vector2.Dot(currentVelocityNormalized, movementInputFromCameraPOV) > turnAroundMaxDotProduct)//si la velocidad y el nuevo input (en referencia a la cámara) están a 120 grados o más -> turn around
{
if (movementInput != Vector2.zero)
{
float step = 11f * Time.deltaTime;
player.transform.forward = Vector3.RotateTowards(player.transform.forward, movement, step, 0.0f);
rigidbody.AddForce(player.transform.forward * powerState.groundAcceleration);//Girar hasta estar en la misma dirección que movement y add force siempre para adelante
}
else
{
if (player.lastMovementInput != Vector2.zero)
{
rigidbody.velocity *= powerState.groundReleaseDeceleration;
}
}
if (isRunning)
{
if (Mathf.Abs(rigidbody.velocity.magnitude) > powerState.maxGroundSpeed)
{
//rigidbody.velocity = Vector3.ClampMagnitude(rigidbody.velocity, powerState.maxGroundSpeed);
rigidbody.velocity = player.transform.forward * powerState.maxGroundSpeed;
}
}
else
{
if (Mathf.Abs(rigidbody.velocity.magnitude) > maxWalkingVelocity)
{
//rigidbody.velocity = Vector3.ClampMagnitude(rigidbody.velocity, maxWalkingVelocity);
rigidbody.velocity = player.transform.forward * maxWalkingVelocity;
}
}
}
else
{
//Debug.Log("Turn around, velocity = " + rigidbody.velocity.magnitude);
player.SetActionState(PlayerModel.ActionStates.TurnAround);
}
}
public bool DoFracture(ref Particle other, ref Particle me)
{
bool somethingBroke = false;
double len = (other.goal - goal).Length();
double rest = (other.x0 - x0).Length();
double off = Math.Abs((len / rest) - 1.0);
if (off > LsmBody.fractureLengthTolerance)
{
somethingBroke = true;
Testbed.PostMessage("Length fracture: Rest = " + rest + ", actual = " + len);
}
if (!somethingBroke)
{
Vector2 a = new Vector2(other.R[0, 0], other.R[1, 0]);
Vector2 b = new Vector2(R[0, 0], R[1, 0]);
a.Normalize();
b.Normalize();
double angleDifference = Math.Acos(a.Dot(b));
if (angleDifference > LsmBody.fractureAngleTolerance)
{
somethingBroke = true;
Testbed.PostMessage("Angle fracture: angle difference = " + angleDifference);
}
}
if (somethingBroke)
{
Particle saved = other;
me = null;
other = null;
// Check if the chunks are still connected
Queue<Particle> edge = new Queue<Particle>();
List<Particle> found = new List<Particle>();
edge.Enqueue(this);
bool connected = false;
while (edge.Count > 0)
{
Particle p = edge.Dequeue();
if (!found.Contains(p))
{
found.Add(p);
if (p == saved)
{
// Connected
connected = true;
break;
}
if (p.xPos != null)
edge.Enqueue(p.xPos);
if (p.xNeg != null)
edge.Enqueue(p.xNeg);
if (p.yPos != null)
edge.Enqueue(p.yPos);
if (p.yNeg != null)
edge.Enqueue(p.yNeg);
}
}
if (connected == false)
{
// The chunks broke - there are now two separate chunks (maximally connected subgraphs)
chunk.particles.Clear();
chunk.particles.AddRange(found);
chunk.CalculateInvariants();
Chunk newChunk = new Chunk();
edge.Clear();
found.Clear();
edge.Enqueue(saved);
while (edge.Count > 0)
{
Particle p = edge.Dequeue();
if (!found.Contains(p))
{
found.Add(p);
p.chunk = newChunk;
if (p.xPos != null)
edge.Enqueue(p.xPos);
if (p.xNeg != null)
edge.Enqueue(p.xNeg);
if (p.yPos != null)
edge.Enqueue(p.yPos);
if (p.yNeg != null)
edge.Enqueue(p.yNeg);
}
}
newChunk.particles.AddRange(found);
newChunk.CalculateInvariants();
body.chunks.Add(newChunk);
Testbed.PostMessage("Chunk broken: the original chunk now has " + chunk.particles.Count + " particles, the new chunk has " + newChunk.particles.Count + " particles.");
Testbed.PostMessage("Number of chunks / particles: " + body.chunks.Count + " / " + body.particles.Count);
}
}
return somethingBroke;
}
public static Vector2 Projection(Vector2 v1, Vector2 v2)
{
return new Vector2(v2 * ((float)(v1.Dot(v2) / Math.Pow(v2.Length, 2))));
}
private Quaternion GetRotationFromCircularMovement()
{
// TODO: use cameraComponent.WorldToScreenPosition instead once implemented
// determine the anchor entity's screen position
var anchorEntityWorldPosition = AnchorEntity.Transform.WorldMatrix.TranslationVector;
var cameraComponent = Game.EditorServices.Get<IEditorGameCameraService>().Component;
Vector3.TransformCoordinate(ref anchorEntityWorldPosition, ref cameraComponent.ViewProjectionMatrix, out var clipSpace);
Vector3.TransformCoordinate(ref anchorEntityWorldPosition, ref cameraComponent.ViewMatrix, out var viewSpace);
var anchorEntityScreenPosition = new Vector2
{
X = (clipSpace.X + 1f) / 2f,
Y = (clipSpace.Y + 1f) / 2f - 1f,
};
// determine the vectors going from the anchor entity's position to the start and current mouse positions
var anchorEntityToMouse = new Vector2(Input.MousePosition.X, -Input.MousePosition.Y) - anchorEntityScreenPosition;
var anchorEntityToStartMouse = new Vector2(StartMousePosition.X, -StartMousePosition.Y) - anchorEntityScreenPosition;
anchorEntityToMouse.X *= cameraComponent.AspectRatio;
anchorEntityToStartMouse.X *= cameraComponent.AspectRatio;
// determine the rotation angle
var rotationAngle = MathF.Atan2(anchorEntityToMouse.X * anchorEntityToStartMouse.Y - anchorEntityToMouse.Y * anchorEntityToStartMouse.X, Vector2.Dot(anchorEntityToStartMouse, anchorEntityToMouse));
// snap the rotation angle if necessary
if (UseSnap)
{
var snapValue = MathUtil.DegreesToRadians(SnapValue);
rotationAngle = MathUtil.Snap(rotationAngle, snapValue);
}
// determine the rotation axis
var rotationAxisWorldUp = rotationAxes[(int)TransformationAxes / 2].Transform.WorldMatrix.Up;
var cameraToAnchorEntity = AnchorEntity.Transform.WorldMatrix.TranslationVector - Game.EditorServices.Get<IEditorGameCameraService>().Position;
var rotationAxis = new Vector3(0) { [(int)TransformationAxes / 2] = MathF.Sign(Vector3.Dot(cameraToAnchorEntity, rotationAxisWorldUp)) };
return Quaternion.RotationAxis(rotationAxis, rotationAngle);
}
public double Qform(Vector2 u, Vector2 v)
{
return u.Dot(this.Mult(v));
}
private static float GetWaveSpectrum( WaveAnimationParameters parameters, Vector2 windDir, Vector2 k )
{
float lenK = k.Length;
if ( lenK < 0.000001f )
{
return 0;
}
Vector2 nK = k / lenK;
float a = parameters.WaveModifier;
float l = parameters.WindSpeed * parameters.WindSpeed / 10;
float lenK2 = lenK * lenK;
float f = ( float )Math.Exp( -1 / ( lenK2 * l * l ) ) / ( lenK2 * lenK2 );
float wDotK = windDir.Dot( nK );
return f * wDotK * wDotK * a;
}
public static Vector2 Projection(Vector2 pBaseVector, Vector2 pProjectedVector)
{
return (pProjectedVector.Dot(pBaseVector) / pBaseVector.Dot(pBaseVector)) * pBaseVector;
}
public void dot() {
var a = new Vector2(5, 2);
var b = new Vector2(3, -3);
Assert.Equal(9, a.Dot(b));
Assert.Equal(9, b.Dot(a));
}
/// <summary>
/// Builds this plane from a line segment
/// </summary>
/// <param name="start">Line start</param>
/// <param name="end">Line end</param>
public Plane2( Point2 start, Point2 end )
{
m_Normal = ( end - start ).MakePerpNormal( );
m_Distance = -m_Normal.Dot( start );
}
public static float Dot(Vector2 vector1, Vector2 vector2)
{
return vector1.Dot(vector2);
}
/// <summary>
/// Calculates the shortest distance from the specified polygon to the specified point,
/// and the axis from polygon to pos.
///
/// Returns null if pt is contained in the polygon (not strictly).
/// </summary>
/// <returns>The distance form poly to pt.</returns>
/// <param name="poly">The polygon</param>
/// <param name="pos">Origin of the polygon</param>
/// <param name="rot">Rotation of the polygon</param>
/// <param name="pt">Point to check.</param>
public static Tuple <Vector2, float> MinDistance(Polygon2 poly, Vector2 pos, Rotation2 rot, Vector2 pt)
{
/*
* Definitions
*
* For each line in the polygon, find the normal of the line in the direction of outside the polygon.
* Call the side of the original line that contains none of the polygon "above the line". The other side is "below the line".
*
* If the point falls above the line:
* Imagine two additional lines that are normal to the line and fall on the start and end, respectively.
* For each of those two lines, call the side of the line that contains the original line "below the line". The other side is "above the line"
*
* If the point is above the line containing the start:
* The shortest vector is from the start to the point
*
* If the point is above the line containing the end:
* The shortest vector is from the end to the point
*
* Otherwise
* The shortest vector is from the line to the point
*
* If this is not true for ANY of the lines, the polygon does not contain the point.
*/
var last = Math2.Rotate(poly.Vertices[poly.Vertices.Length - 1], poly.Center, rot) + pos;
for (var i = 0; i < poly.Vertices.Length; i++)
{
var curr = Math2.Rotate(poly.Vertices[i], poly.Center, rot) + pos;
var axis = curr - last;
Vector2 norm;
if (poly.Clockwise)
{
norm = new Vector2(-axis.Y, axis.X);
}
else
{
norm = new Vector2(axis.Y, -axis.X);
}
norm = Vector2.Normalize(norm);
axis = Vector2.Normalize(axis);
var lineProjOnNorm = Vector2.Dot(norm, last);
var ptProjOnNorm = Vector2.Dot(norm, pt);
if (ptProjOnNorm > lineProjOnNorm)
{
var ptProjOnAxis = Vector2.Dot(axis, pt);
var stProjOnAxis = Vector2.Dot(axis, last);
if (ptProjOnAxis < stProjOnAxis)
{
var res = pt - last;
return(Tuple.Create(Vector2.Normalize(res), res.Length()));
}
var enProjOnAxis = Vector2.Dot(axis, curr);
if (ptProjOnAxis > enProjOnAxis)
{
var res = pt - curr;
return(Tuple.Create(Vector2.Normalize(res), res.Length()));
}
var distOnNorm = ptProjOnNorm - lineProjOnNorm;
return(Tuple.Create(norm, distOnNorm));
}
last = curr;
}
return(null);
}
internal void CalcTurnin()
{
//GameObject goCirc;
//CapsuleCollider Circ;
//goCirc = GameObject.CreatePrimitive(PrimitiveType.Cube);
//goCirc.GetComponent<BoxCollider>().enabled = false;
//Circ = goCirc.AddComponent<CapsuleCollider>();
//goCirc.name = "CircColl" + BendId;
//Circ.height = 100;
Radius = SqrtRad * SqrtRad;
Vector3 Centre = new Vector3(0, 0, 0);
Vector3 C = new Vector3(0, 0, 0);
Vector3 AP = VectGeom.Convert2d(ApexPos);
Vector3 TP = new Vector3(0, 0, 0);
Vector3 EP = new Vector3(0, 0, 0);
int e = 0;
float ExitFrac = 1;//1 is a wide turn, 0 is tight
float SmallestRadErr = 1000;
PossBendCircle bbc = new PossBendCircle();
Vector3 ApexPos2d = VectGeom.Convert2d(ApexPos);
float NxtBndDist = Rd.XSecs.Diff(ApexXSec, NextBend.ApexXSec);
//Method:
//Try a range of turnin segs
//For each Turnin seg, draw a line perpendicular to the fwd direction
//and find the turninPt (TP)
//Find the midpoint (MP) of the line from apex to turninPoint and draw a line perpendicular to this (MPerp)
//Where MPerp crosses TPerp is the centre of the circle, C
/// <image url="$(SolutionDir)\CommonImages\CalcTurninAlgorithm.png" scale="1.2"/>
if (Type == BendType.Right)
{
if (NextBend.Type == BendType.Left && NxtBndDist < 200)
{
ExitFrac = NxtBndDist / 200;
}
for (XSec tx = Rd.XSecs[StartSegIdx - 70]; tx.IsBefore(Rd.XSecs[StartSegIdx]); tx = Rd.XSecs.Next(tx)) //removed circlebug
{
Vector2 TPerp = VectGeom.Convert2d(Vector3.Cross(Vector3.up, tx.Forward));
TP = VectGeom.Convert2d(tx.KerbL + (tx.KerbR - tx.KerbL).normalized * TurninGap);
Vector2 MP = (ApexPos2d + TP) / 2;
Vector2 MPerp = new Vector2((ApexPos2d - TP).y, -(ApexPos2d - TP).x);
if (VectGeom.LineLineIntersection(out C, TP, TPerp, MP, MPerp)) //these vars are all Vector3(x,0,y)
{
float biggestCos = 0;
float R = Vector2.Distance(TP, C);
PossBendCircle pbc = new PossBendCircle();
float RadErr = 1000;
for (e = EndXSec.Idx + 70; e > EndXSec.Idx; e--) //bugbugbug circlebug
{
Vector2 WideExitPt = VectGeom.Convert2d(Rd.XSecs[e].KerbL + (Rd.XSecs[e].KerbR - Rd.XSecs[e].KerbL).normalized * 2);
Vector2 TightExitPoint = VectGeom.Convert2d(Rd.XSecs[e].KerbR + (Rd.XSecs[e].KerbL - Rd.XSecs[e].KerbR).normalized * 2);
EP = Vector2.Lerp(TightExitPoint, WideExitPt, ExitFrac);
Vector3 EPerp = VectGeom.Convert2d(-Rd.XSecs[e].Right).normalized;
float cos = Mathf.Abs(Vector2.Dot((EP - C).normalized, EPerp));
if (cos > biggestCos)
{
RadErr = Mathf.Abs(Vector3.Distance(EP, C) - R);
pbc = new PossBendCircle {
C = C, RSq = 0, RadErr = RadErr, EP = EP, EX = Rd.XSecs[e], TP = TP, TX = tx
};
biggestCos = cos;
}
}
if (RadErr < SmallestRadErr)
{
bbc = new PossBendCircle {
C = pbc.C, RSq = 0, RadErr = pbc.RadErr, EP = pbc.EP, EX = pbc.EX, TP = pbc.TP, TX = pbc.TX
};
SmallestRadErr = pbc.RadErr;
}
}
}
Centre = new Vector3(bbc.C.x, ApexPos.y, bbc.C.y);
Radius = Vector2.Distance(bbc.TP, bbc.C);
TurninXSec = bbc.TX;
TurninSegIdx = TurninXSec.Idx;
TurninPos = new Vector3(bbc.TP.x, ApexPos.y, bbc.TP.y);
ExitXSec = bbc.EX;
ExitPos = new Vector3(bbc.EP.x, ApexPos.y, bbc.EP.y);
goto FoundCentre;
//Not found Centre
Debug.Log("No Centre for Bend" + BendId);
ExitXSec = Rd.XSecs[e];
Debug.Break();
}
if (Type == BendType.Left)
{
if (NextBend.Type == BendType.Right && NxtBndDist < 200)
{
ExitFrac = NxtBndDist / 200;
}
for (int t = StartSegIdx - 70; t < StartSegIdx; t++) //bugbugbug circlebug
{
Vector2 TPerp = VectGeom.Convert2d(Vector3.Cross(Vector3.up, Rd.XSecs[t].Forward));
TP = VectGeom.Convert2d(Rd.XSecs[t].KerbR + (Rd.XSecs[t].KerbL - Rd.XSecs[t].KerbR).normalized * TurninGap);
Vector2 MP = (ApexPos2d + TP) / 2;
Vector2 MPerp = new Vector2((ApexPos2d - TP).y, -(ApexPos2d - TP).x);
if (VectGeom.LineLineIntersection(out C, TP, TPerp, MP, MPerp)) //these vars are all Vector3(x,0,y)
{
float biggestCos = 0;
float R = Vector2.Distance(TP, C);
PossBendCircle pbc = new PossBendCircle();
float RadErr = 1000;
for (e = EndXSec.Idx + 70; e > EndXSec.Idx; e--) //bugbugbug circlebug
{
Vector2 WideExitPt = VectGeom.Convert2d(Rd.XSecs[e].KerbR + (Rd.XSecs[e].KerbL - Rd.XSecs[e].KerbR).normalized * 2);
Vector2 TightExitPoint = VectGeom.Convert2d(Rd.XSecs[e].KerbL + (Rd.XSecs[e].KerbR - Rd.XSecs[e].KerbL).normalized * 2);
EP = Vector2.Lerp(TightExitPoint, WideExitPt, ExitFrac);
Vector3 EPerp = VectGeom.Convert2d(Rd.XSecs[e].Right).normalized;
float cos = Mathf.Abs(Vector2.Dot((EP - C).normalized, EPerp));
if (cos > biggestCos)
{
RadErr = Mathf.Abs(Vector3.Distance(EP, C) - R);
pbc = new PossBendCircle {
C = C, RSq = 0, RadErr = RadErr, EP = EP, EX = Rd.XSecs[e], TP = TP, TX = Rd.XSecs[t]
};
biggestCos = cos;
}
}
if (RadErr < SmallestRadErr)
{
bbc = new PossBendCircle {
C = pbc.C, RSq = 0, RadErr = pbc.RadErr, EP = pbc.EP, EX = pbc.EX, TP = pbc.TP, TX = pbc.TX
};
SmallestRadErr = pbc.RadErr;
}
}
}
Centre = new Vector3(bbc.C.x, ApexPos.y, bbc.C.y);
Radius = Vector2.Distance(bbc.TP, bbc.C);
TurninXSec = bbc.TX;
TurninSegIdx = TurninXSec.Idx;
TurninPos = new Vector3(bbc.TP.x, ApexPos.y, bbc.TP.y);
ExitXSec = bbc.EX;
ExitPos = new Vector3(bbc.EP.x, ApexPos.y, bbc.EP.y);
goto FoundCentre;
//Not found Centre
Debug.Log("No Centre for Bend" + BendId);
ExitXSec = Rd.XSecs[e];
Debug.Break();
}
FoundCentre:
//goCirc.transform.position = Centre;
//Circ.radius = Radius;
Angle = VectGeom.SignedAngle(TurninXSec.Forward, ExitXSec.Forward);
float RacelineSegCount = Rd.XSecs.Diff(TurninXSec, ExitXSec);;
RacelineAnglePerSeg = Angle / RacelineSegCount;
RacelineSegLength = Angle * Mathf.Deg2Rad * Radius / RacelineSegCount;
CalculateSpeed();
//Circ.enabled = false; //cos the gameobject doesnt get destroyed till end of frame
//GameObject.Destroy(goCirc);
return;
NoTurnin:
if (TurninXSec == null)
{
TurninXSec = Rd.XSecs[ApexSegIdx - 50]; TurninSegIdx = TurninXSec.Idx;
ExitXSec = Rd.XSecs[ApexSegIdx + (ApexSegIdx - TurninSegIdx)];
if (Type == BendType.Right)
{
TurninPos = TurninXSec.KerbL + (TurninXSec.KerbR - TurninXSec.KerbL).normalized * 4f;
}
else
{
TurninPos = TurninXSec.KerbR + (TurninXSec.KerbL - TurninXSec.KerbR).normalized * 4f;
}
Radius = 100;
AnalyseTurnin();
CalculateSpeed();
}
//Circ.enabled = false; //cos the gameobject doesnt get destroyed till end of frame
//GameObject.Destroy(goCirc);
}
public void OrthoNormalise(Vector2 u, Vector2 v)
{
// If the input vectors are v0 and v1, then the Gram-Schmidt
// orthonormalization produces vectors u0 and u1 as follows,
//
// u0 = v0/|v0|
// u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0|
//
// where |A| indicates length of vector A and A*B indicates dot
// product of vectors A and B.
// compute u0
u.Normalise();
// compute u1
double dot0 = u.Dot(v);
v = v - u * dot0;
v.Normalise();
}
/// <summary>
/// Projects the point q onto a line defined by the points lineA and lineB.
/// </summary>
/// <param name="q">The point q.</param>
/// <param name="lineA">The point lineA which defines a line with lineB.</param>
/// <param name="lineB">The point lineB which defines a line with lineA.</param>
public static Vector2 Project(Vector2 q, Vector2 lineA, Vector2 lineB)
{
Vector2 ab = lineB - lineA;
return(lineA + Vector2.Dot(q - lineA, ab) / Vector2.Dot(ab, ab) * ab);
}
/// <summary>
/// Updates the ANN with information from the sweepers enviroment
/// First we take sensor readings and feed these into the sweepers brain.
///
/// The inputs are:
///
/// A vector to the closest mine (x, y)
/// The sweepers 'look at' vector (x, y)
///
/// We receive two outputs from the brain.. lTrack & rTrack.
/// So given a force for each track we calculate the resultant rotation
/// and acceleration and apply to current velocity vector.
/// </summary>
/// <param name="mines"></param>
/// <returns></returns>
public bool UpdateANN2(List <Vector2> mines)
{
this.acceleration = Vector2.zero;
this.velocity = Vector2.zero;
//this will store all the inputs for the NN
List <double> inputs = new List <double>();
//this.Rotation = UnityEngine.Random.Range(0, AIConstants.TWO_PI);
/*
* First of all, the function calculates a vector to the closest mine and then normalizes
* it. (Remember, when a vector is normalized its length becomes 1.) The
* minesweeper’s look-at vector doesn’t need to be normalized in this way because its
* length is always 1. Because both vectors have been effectively scaled to within the
* same limits, the inputs can be considered to be standardized.
*/
//get vector to closest mine
Vector2 vectorClosestMine = GetClosestMine(mines);
//normalise it
vectorClosestMine.Normalize();
float dot = Vector2.Dot(this.LookAt, vectorClosestMine);
int sign = -1;
if (this.LookAt.y * vectorClosestMine.x > this.LookAt.x * vectorClosestMine.y)
{
sign = 1;
}
else
{
sign = -1;
}
/*
* The look-at vector and the vector to the closest mine are then input into the neural
* network. The NeuralNet Update function updates the minesweeper’s network with
* this information and returns a List of doubles as the output.
*/
//add in vector to closest mine
//inputs.Add(vectorClosestMine.x);
//inputs.Add(vectorClosestMine.y);
//add in sweepers look at vector
//inputs.Add(this.LookAt.x);
//inputs.Add(this.LookAt.y);
//if (HasHitObstacle)
//{
// int y = 0;
// Debug.Log("#####################################################");
// Debug.Log(dot);
// Debug.Log("item count: " + this.sweeperCollisionSensorData.Count);
// foreach (double d in this.sweeperCollisionSensorData)
// Debug.Log("sw: " + d);
//}
inputs.Add(dot * sign);
inputs.AddRange(this.sweeperCollisionSensorData);
inputs.AddRange(this.sweeperMemoryMapSensorData);
//update the brain and get feedback
List <double> output = this.Brain.Update(inputs);
//make sure there were no errors in calculating the
//output
if (output.Count < NeuralNetworkParams.NumOutputs)
{
return(false);
}
//assign the outputs to the sweepers left & right tracks
this.LeftTrack = output[0];
this.RightTrack = output[1];
/*
* After checking to make sure there are no errors when updating the neural network,
* the program assigns the outputs to LeftTrack and RightTrack. These values represent the
* forces being exerted on the left track and right track of the minesweeper.
*/
//calculate steering forces
double RotForce = this.LeftTrack - this.RightTrack;
//clamp rotation
Mathf.Clamp((float)RotForce, -(float)NeuralNetworkParams.MaxTurnRate, (float)NeuralNetworkParams.MaxTurnRate);
/*
* The vehicle’s rotational force is calculated by subtracting the force exerted by the
* right track from the force exerted by the left track. This is then clamped to make
* sure it doesn’t exceed the maximum turn rate specified in the ini file. The vehicle’s
* speed is simply the sum of the left track and right track. Now that we know the
* minesweeper’s rotational force and speed, its position and rotation can be updated
* accordingly.
*/
//update the minesweepers rotation
this.Rotation += RotForce;
this.Speed = (this.LeftTrack + this.RightTrack);
this.Speed *= NeuralNetworkParams.MaxSpeed;
//this.Speed *= 0.5f;
//update Look At
this.LookAt.x = Mathf.Cos((float)this.Rotation);
this.LookAt.y = -Mathf.Sin((float)this.Rotation);
this.acceleration += (this.LookAt * (float)this.Speed);
this.velocity += this.acceleration;
//var rot = this.transform.rotation.eulerAngles;
//rot.z += (float)Rotation;
//transform.rotation = Quaternion.Euler(rot);
//if (this.velocity.x > NeuralNetworkParams.MaxSpeed)
// this.velocity.x = (float)NeuralNetworkParams.MaxSpeed;
//if (this.velocity.x < -NeuralNetworkParams.MaxSpeed)
// this.velocity.x = -(float)NeuralNetworkParams.MaxSpeed;
//if (this.velocity.y > (float)NeuralNetworkParams.MaxSpeed)
// this.velocity.y = (float)NeuralNetworkParams.MaxSpeed;
//if (this.velocity.y < -(float)NeuralNetworkParams.MaxSpeed)
// this.velocity.y = -(float)NeuralNetworkParams.MaxSpeed;
var previousPosition = this.transform.position;
this.transform.position += (Vector3)this.velocity;
var movementThisStep = this.transform.position - previousPosition;
var movementLastStep = previousPosition - this.sweeperLastPosition;
var previousPositionContinuedDirection = (previousPosition + movementLastStep) - previousPosition;
var forward = transform.forward;
float angle = Vector3.Angle(movementThisStep, previousPositionContinuedDirection);
//this.transform.Rotate(Vector3.forward, angle);
// Rotate the object towards the movement direction. NOTICE: the -90 degree turn is to point the sprite to the right direction
transform.rotation = Quaternion.Euler(new Vector3(0, 0, (Mathf.Atan2(movementThisStep.y, movementThisStep.x) * Mathf.Rad2Deg) + -90));
//this.transform.LookAt(this.transform.position + movementThisStep, Vector3.left);
//this.rig2d.MoveRotation(angle);
//var rot = this.transform.rotation.eulerAngles;
////var rot2 = Quaternion.LookRotation(movementThisStep.normalized);
////rot2 *= rot;
//var fw = this.transform.forward;
//rot.z += angle;
//transform.rotation = Quaternion.
var movementThisStepSensorLength = movementThisStep * 7;
//float movementSqrMagnitude = movementThisStepSensorLength.sqrMagnitude;
//float movementMagnitude = Mathf.Sqrt(movementSqrMagnitude);
//RaycastHit2D hitInfo = Physics2D.Raycast(previousPosition, movementThisStepSensorLength, movementMagnitude, layerMask.value);
//if (hitInfo.collider)
//{
// var collisionRay = hitInfo.point - new Vector2(previousPosition.x, previousPosition.y);
// var ratio = collisionRay.sqrMagnitude / movementSqrMagnitude;
// if (ratio <= this.CollisionDistance)
// {
// this.sweeperCollisionSensorData.Add(ratio);
// this.HasHitObstacle = true;
// } else
// {
// this.sweeperCollisionSensorData.Add(-1);
// this.HasHitObstacle = false;
// }
//}
this.DetectObstacles(previousPosition, movementThisStepSensorLength);
this.ProcessMemoryMapWithSensors(previousPosition, movementThisStepSensorLength);
if (this.GetComponent <SpriteRenderer>().color == Color.red || this.GetComponent <SpriteRenderer>().color == Color.yellow || this.GetComponent <SpriteRenderer>().color == Color.magenta)
{
Debug.DrawRay(previousPosition, movementThisStep, this.GetComponent <SpriteRenderer>().color, 1.7f);
// Draw rays from current new position forward, this can be used for collision detection
//--------------------------------------------------------
// Points forward
Debug.DrawRay(this.transform.position, movementThisStepSensorLength, Color.green);
var sensorIn45DegreeForwardAngle = Quaternion.Euler(0, 0, 45) * movementThisStepSensorLength;
var sensorInMinus45DegreeForwardAngle = Quaternion.Euler(0, 0, -45) * movementThisStepSensorLength;
// Point 45 degrees forward
Debug.DrawRay(this.transform.position, sensorIn45DegreeForwardAngle, Color.cyan);
Debug.DrawRay(this.transform.position, sensorInMinus45DegreeForwardAngle, Color.cyan);
var sensorIn90DegreeForwardAngle = Quaternion.Euler(0, 0, 90) * movementThisStepSensorLength;
var sensorInMinus90DegreeForwardAngle = Quaternion.Euler(0, 0, -90) * movementThisStepSensorLength;
// Points to the side of the object
Debug.DrawRay(this.transform.position, sensorIn90DegreeForwardAngle, Color.yellow);
Debug.DrawRay(this.transform.position, sensorInMinus90DegreeForwardAngle, Color.yellow);
// Points backwards
Debug.DrawRay(this.transform.position, movementThisStepSensorLength * -1, Color.red);
//--------------------------------------------------------
}
//update position
//this.transform.LookAt(this.LookAt, Vector3.zero);
//this.rig2d.velocity += (this.LookAt * (float)this.Speed);
//this.rig2d.velocity = (this.LookAt * (float)this.Speed);
//this.rig2d.MoveRotation((float)this.Rotation);
//this.rig2d.AddForce((this.LookAt * (float)this.Speed));
//this.transform.position += new Vector3(this.LookAt.x , this.LookAt.y );
//this.transform.position += new Vector3(this.LookAt.x * (float)this.Speed, this.LookAt.y * (float)this.Speed);
//this.rig2d.AddForce(transform.up * (float)this.Speed);
//wrap around window limits
if (this.transform.position.x > maxScreenTopRight.x)
{
this.transform.position = new Vector3(minScreenBottomLeft.x, this.transform.position.y, 0);
}
if (this.transform.position.x < minScreenBottomLeft.x)
{
this.transform.position = new Vector3(maxScreenTopRight.x, this.transform.position.y, 0);
}
if (this.transform.position.y > maxScreenTopRight.y)
{
this.transform.position = new Vector3(this.transform.position.x, minScreenBottomLeft.y, 0);
}
if (this.transform.position.y < minScreenBottomLeft.y)
{
this.transform.position = new Vector3(this.transform.position.x, maxScreenTopRight.y, 0);
}
this.sweeperLastPosition = previousPosition;
this.MemoryMap.Update(this.transform.position.x, this.transform.position.y);
return(true);
}
private CollisionSubframeBuffer GenerateContact_Reflection(
Particle particle, Particle origin, Particle neighbor, Particle.CCDDebugInfo ccd, double ccdCollisionTime, double timeCoefficientPrediction,
CollisionSubframeBuffer subframeToAdd
)
{
double alpha = ccd.coordinateOfPointOnEdge;
Vector2 velocityEdgeCollisionPoint = origin.v + (neighbor.v - origin.v) * alpha;
Vector2 velocityPointRelativeEdge = particle.v - velocityEdgeCollisionPoint;
// compute velocity reflection relativly moving edge
Vector2 reflectSurface = ccd.edge.end - ccd.edge.start;
Vector2 reflectNormal = new Vector2(-reflectSurface.Y, reflectSurface.X);
if (reflectNormal.Dot(velocityPointRelativeEdge) < 0) reflectNormal = -reflectNormal;
Vector2 newVelocity =
velocityPointRelativeEdge - (1.0 + coefficientElasticity) * reflectNormal * (reflectNormal.Dot(velocityPointRelativeEdge) / reflectNormal.LengthSq());
if (ccdCollisionTime <= 0.0) Testbed.PostMessage(System.Drawing.Color.Red, "timeCoefficient = 0"); // Zero-Distance not allowed // DEBUG
double newTimeCoefficient = timeCoefficientPrediction * ccdCollisionTime;
newTimeCoefficient -= epsilon / newVelocity.Length(); // try to prevent Zero-Distances // HACK // TODO: check Length() > epsilon
if (newTimeCoefficient < 0.0) newTimeCoefficient = 0.0; // don't move particle toward edge - just reflect velocity
newVelocity += velocityEdgeCollisionPoint; // newVelocity should be in global coordinates
subframeToAdd.vParticle = newVelocity;
subframeToAdd.timeCoefficient = newTimeCoefficient;
return subframeToAdd;
}
/// <summary>
/// 计算射线 和圆的两个相交点
/// 得到较近的那个点
/// </summary>
/// <param name="gPos"></param>
/// <param name="firstCol"></param>
/// <param name="gridDir"></param>
/// <param name="startPos"></param>
/// <returns></returns>
private bool FixRaycastPos(Vector2 gPos, Vector2 firstCol, Vector2 gridDir, Vector2 startPos, out Vector2 fixPos)
{
var totalRadius = playerRadius + outGridRadius;
var C = firstCol;
var P = startPos;
var D = gPos - startPos;
var R = totalRadius;
var Delta = P - C;
var DD = Vector2.Dot(D, Delta);
var DSqr = Vector2.Dot(D, D);
var CigMa = DD * DD - DSqr * (Vector2.Dot(Delta, Delta) - R * R);
fixPos = Vector2.zero;
if (CigMa < 0)
{
fixPos = gPos;
return(false);
}
var sqrt = Mathf.Sqrt(CigMa);
var t1 = (-DD + sqrt) / DSqr;
var t2 = (-DD - sqrt) / DSqr;
var t1Ok = false;
var t2Ok = false;
//t->[0, 1]
if (t1 > 0 && t1 < 1)
{
t1Ok = true;
}
if (t2 > 0 && t2 < 1)
{
t2Ok = true;
}
if (t1Ok && t2Ok)
{
var t = Mathf.Min(t1, t2);
fixPos = P + (t - TEPSILON) * D;
return(true);
}
if (t1Ok)
{
var t = t1;
fixPos = P + (t - TEPSILON) * D;
return(true);
}
if (t2Ok)
{
var t = t2;
fixPos = P + (t - TEPSILON) * D;
return(true);
}
//留在原地不要动
fixPos = P;
return(false);
}
private static void DispatchPolygonToPolygon(Manifold Manifold)
{
PolygonShape a = (PolygonShape)Manifold.BodyA.Shape;
PolygonShape b = (PolygonShape)Manifold.BodyB.Shape;
Manifold.Points = null;
// Check for a separating axis with A's face planes
uint faceA;
Number penetrationA;
FindAxisLeastPenetration(Manifold.BodyA, Manifold.BodyB, out faceA, out penetrationA);
if (penetrationA >= 0)
{
return;
}
// Check for a separating axis with B's face planes
uint faceB;
Number penetrationB;
FindAxisLeastPenetration(Manifold.BodyB, Manifold.BodyA, out faceB, out penetrationB);
if (penetrationB >= 0)
{
return;
}
uint referenceIndex;
bool flip; // Always point from a to b
Body refBody;
Body incBody;
PolygonShape refPoly;
PolygonShape incPoly;
// Determine which shape contains reference face
if (Math.BiasGreaterThan(penetrationA, penetrationB))
{
refBody = Manifold.BodyA;
refPoly = a;
incBody = Manifold.BodyB;
incPoly = b;
referenceIndex = faceA;
flip = false;
}
else
{
refBody = Manifold.BodyB;
refPoly = b;
incBody = Manifold.BodyA;
incPoly = a;
referenceIndex = faceB;
flip = true;
}
Matrix2 refBodyOrientation = refBody.Orientation;
// World space incident face
Vector2[] incidentFace = new Vector2[2];
FindIncidentFace(incidentFace, refBody, refPoly, incBody, incPoly, referenceIndex);
// y
// ^ ->n ^
// +---c ------posPlane--
// x < | i |\
// +---+ c-----negPlane--
// \ v
// r
//
// r : reference face
// i : incident poly
// c : clipped point
// n : incident normal
// Setup reference face vertices
Vector2 v1 = refPoly.Vertices[referenceIndex];
referenceIndex = referenceIndex + 1 == refPoly.Vertices.Length ? 0 : referenceIndex + 1;
Vector2 v2 = refPoly.Vertices[referenceIndex];
// Transform vertices to world space
v1 = refBodyOrientation * v1 + refBody.Position;
v2 = refBodyOrientation * v2 + refBody.Position;
// Calculate reference face side normal in world space
Vector2 sidePlaneNormal = (v2 - v1);
sidePlaneNormal.Normalize();
// Orthogonalize
Vector2 refFaceNormal = new Vector2(sidePlaneNormal.Y, -sidePlaneNormal.X);
// ax + by = c
// c is distance from origin
Number refC = refFaceNormal.Dot(v1);
Number negSide = -sidePlaneNormal.Dot(v1);
Number posSide = sidePlaneNormal.Dot(v2);
// Clip incident face to reference face side planes
if (Clip(-sidePlaneNormal, negSide, incidentFace) < 2)
{
return; // Due to floating point error, possible to not have required points
}
if (Clip(sidePlaneNormal, posSide, incidentFace) < 2)
{
return; // Due to floating point error, possible to not have required points
}
// Flip
Manifold.Normal = refFaceNormal * (flip ? -1 : 1);
// Keep points behind reference face
uint cp = 0; // clipped points behind reference face
Number separation = refFaceNormal.Dot(incidentFace[0]) - refC;
if (separation <= 0)
{
ArrayUtilities.Add(ref Manifold.Points, incidentFace[0]);
Manifold.Penetration = -separation;
++cp;
}
else
{
Manifold.Penetration = 0;
}
separation = refFaceNormal.Dot(incidentFace[1]) - refC;
if (separation <= 0)
{
ArrayUtilities.Add(ref Manifold.Points, incidentFace[1]);
Manifold.Penetration += -separation;
++cp;
// Average penetration
Manifold.Penetration /= (int)cp;
}
}
public static Vector2 Reflect(Vector2 vector, Vector2 normal)
{
return(vector - 2 * Vector2.Dot(vector, normal) * normal);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
if (_state == LimitState.AtUpper)
{
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2);
float impulse = _pulleyMass * (-Cdot);
float oldImpulse = _impulse;
_impulse = Math.Max(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -_ratio * impulse * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpper)
{
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
float Cdot = -Vector2.Dot(_u1, v1);
float impulse = -_limitMass1 * Cdot;
float oldImpulse = _limitImpulse1;
_limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
impulse = _limitImpulse1 - oldImpulse;
Vector2 P1 = -impulse * _u1;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpper)
{
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = -Vector2.Dot(_u2, v2);
float impulse = -_limitMass2 * Cdot;
float oldImpulse = _limitImpulse2;
_limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
impulse = _limitImpulse2 - oldImpulse;
Vector2 P2 = -impulse * _u2;
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
}
/// <summary>
/// Activate the explosion at the specified position.
/// </summary>
/// <param name="pos">The position where the explosion happens </param>
/// <param name="radius">The explosion radius </param>
/// <param name="maxForce">The explosion force at the explosion point (then is inversely proportional to the square of the distance)</param>
/// <returns>A list of bodies and the amount of force that was applied to them.</returns>
public Dictionary <Fixture, Vector2> activate(Vector2 pos, float radius, float maxForce)
{
AABB aabb;
aabb.lowerBound = pos + new Vector2(-radius, -radius);
aabb.upperBound = pos + new Vector2(radius, radius);
Fixture[] shapes = new Fixture[maxShapes];
// More than 5 shapes in an explosion could be possible, but still strange.
Fixture[] containedShapes = new Fixture[5];
bool exit = false;
int shapeCount = 0;
int containedShapeCount = 0;
// Query the world for overlapping shapes.
world.queryAABB(
fixture =>
{
if (fixture.testPoint(ref pos))
{
if (ignoreWhenInsideShape)
{
exit = true;
return(false);
}
containedShapes[containedShapeCount++] = fixture;
}
else
{
shapes[shapeCount++] = fixture;
}
// Continue the query.
return(true);
}, ref aabb);
if (exit)
{
return(new Dictionary <Fixture, Vector2>());
}
Dictionary <Fixture, Vector2> exploded = new Dictionary <Fixture, Vector2>(shapeCount + containedShapeCount);
// Per shape max/min angles for now.
float[] vals = new float[shapeCount * 2];
int valIndex = 0;
for (int i = 0; i < shapeCount; ++i)
{
PolygonShape ps;
CircleShape cs = shapes[i].shape as CircleShape;
if (cs != null)
{
// We create a "diamond" approximation of the circle
Vertices v = new Vertices();
Vector2 vec = Vector2.Zero + new Vector2(cs.radius, 0);
v.Add(vec);
vec = Vector2.Zero + new Vector2(0, cs.radius);
v.Add(vec);
vec = Vector2.Zero + new Vector2(-cs.radius, cs.radius);
v.Add(vec);
vec = Vector2.Zero + new Vector2(0, -cs.radius);
v.Add(vec);
ps = new PolygonShape(v, 0);
}
else
{
ps = shapes[i].shape as PolygonShape;
}
if ((shapes[i].body.bodyType == BodyType.Dynamic) && ps != null)
{
Vector2 toCentroid = shapes[i].body.getWorldPoint(ps.massData.centroid) - pos;
float angleToCentroid = (float)Math.Atan2(toCentroid.Y, toCentroid.X);
float min = float.MaxValue;
float max = float.MinValue;
float minAbsolute = 0.0f;
float maxAbsolute = 0.0f;
for (int j = 0; j < ps.vertices.Count; ++j)
{
Vector2 toVertex = (shapes[i].body.getWorldPoint(ps.vertices[j]) - pos);
float newAngle = (float)Math.Atan2(toVertex.Y, toVertex.X);
float diff = (newAngle - angleToCentroid);
diff = (diff - MathHelper.Pi) % (2 * MathHelper.Pi);
// the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be
if (diff < 0.0f)
{
diff += 2 * MathHelper.Pi; // correction for not handling negs
}
diff -= MathHelper.Pi;
if (Math.Abs(diff) > MathHelper.Pi)
{
continue; // Something's wrong, point not in shape but exists angle diff > 180
}
if (diff > max)
{
max = diff;
maxAbsolute = newAngle;
}
if (diff < min)
{
min = diff;
minAbsolute = newAngle;
}
}
vals[valIndex] = minAbsolute;
++valIndex;
vals[valIndex] = maxAbsolute;
++valIndex;
}
}
Array.Sort(vals, 0, valIndex, _rdc);
_data.Clear();
bool rayMissed = true;
for (int i = 0; i < valIndex; ++i)
{
Fixture fixture = null;
float midpt;
int iplus = (i == valIndex - 1 ? 0 : i + 1);
if (vals[i] == vals[iplus])
{
continue;
}
if (i == valIndex - 1)
{
// the single edgecase
midpt = (vals[0] + MathHelper.Pi * 2 + vals[i]);
}
else
{
midpt = (vals[i + 1] + vals[i]);
}
midpt = midpt / 2;
Vector2 p1 = pos;
Vector2 p2 = radius * new Vector2((float)Math.Cos(midpt), (float)Math.Sin(midpt)) + pos;
// RaycastOne
bool hitClosest = false;
world.rayCast((f, p, n, fr) =>
{
Body body = f.body;
if (!isActiveOn(body))
{
return(0);
}
hitClosest = true;
fixture = f;
return(fr);
}, p1, p2);
//draws radius points
if ((hitClosest) && (fixture.body.bodyType == BodyType.Dynamic))
{
if ((_data.Any()) && (_data.Last().Body == fixture.body) && (!rayMissed))
{
int laPos = _data.Count - 1;
ShapeData la = _data[laPos];
la.Max = vals[iplus];
_data[laPos] = la;
}
else
{
// make new
ShapeData d;
d.Body = fixture.body;
d.Min = vals[i];
d.Max = vals[iplus];
_data.Add(d);
}
if ((_data.Count > 1) &&
(i == valIndex - 1) &&
(_data.Last().Body == _data.First().Body) &&
(_data.Last().Max == _data.First().Min))
{
ShapeData fi = _data[0];
fi.Min = _data.Last().Min;
_data.RemoveAt(_data.Count - 1);
_data[0] = fi;
while (_data.First().Min >= _data.First().Max)
{
fi.Min -= MathHelper.Pi * 2;
_data[0] = fi;
}
}
int lastPos = _data.Count - 1;
ShapeData last = _data[lastPos];
while ((_data.Count > 0) &&
(_data.Last().Min >= _data.Last().Max)) // just making sure min<max
{
last.Min = _data.Last().Min - 2 * MathHelper.Pi;
_data[lastPos] = last;
}
rayMissed = false;
}
else
{
rayMissed = true; // raycast did not find a shape
}
}
for (int i = 0; i < _data.Count; ++i)
{
if (!isActiveOn(_data[i].Body))
{
continue;
}
float arclen = _data[i].Max - _data[i].Min;
float first = MathHelper.Min(maxEdgeOffset, edgeRatio * arclen);
int insertedRays = (int)Math.Ceiling(((arclen - 2.0f * first) - (minRays - 1) * maxAngle) / maxAngle);
if (insertedRays < 0)
{
insertedRays = 0;
}
float offset = (arclen - first * 2.0f) / ((float)minRays + insertedRays - 1);
//Note: This loop can go into infinite as it operates on floats.
//Added FloatEquals with a large epsilon.
for (float j = _data[i].Min + first;
j < _data[i].Max || MathUtils.floatEquals(j, _data[i].Max, 0.0001f);
j += offset)
{
Vector2 p1 = pos;
Vector2 p2 = pos + radius * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
Vector2 hitpoint = Vector2.Zero;
float minlambda = float.MaxValue;
List <Fixture> fl = _data[i].Body.fixtureList;
for (int x = 0; x < fl.Count; x++)
{
Fixture f = fl[x];
RayCastInput ri;
ri.Point1 = p1;
ri.Point2 = p2;
ri.MaxFraction = 50f;
RayCastOutput ro;
if (f.rayCast(out ro, ref ri, 0))
{
if (minlambda > ro.Fraction)
{
minlambda = ro.Fraction;
hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
}
}
// the force that is to be applied for this particular ray.
// offset is angular coverage. lambda*length of segment is distance.
float impulse = (arclen / (minRays + insertedRays)) * maxForce * 180.0f / MathHelper.Pi * (1.0f - Math.Min(1.0f, minlambda));
// We Apply the impulse!!!
Vector2 vectImp = Vector2.Dot(impulse * new Vector2((float)Math.Cos(j), (float)Math.Sin(j)), -ro.Normal) * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
_data[i].Body.applyLinearImpulse(ref vectImp, ref hitpoint);
// We gather the fixtures for returning them
if (exploded.ContainsKey(f))
{
exploded[f] += vectImp;
}
else
{
exploded.Add(f, vectImp);
}
if (minlambda > 1.0f)
{
hitpoint = p2;
}
}
}
}
// We check contained shapes
for (int i = 0; i < containedShapeCount; ++i)
{
Fixture fix = containedShapes[i];
if (!isActiveOn(fix.body))
{
continue;
}
float impulse = minRays * maxForce * 180.0f / MathHelper.Pi;
Vector2 hitPoint;
CircleShape circShape = fix.shape as CircleShape;
if (circShape != null)
{
hitPoint = fix.body.getWorldPoint(circShape.position);
}
else
{
PolygonShape shape = fix.shape as PolygonShape;
hitPoint = fix.body.getWorldPoint(shape.massData.centroid);
}
Vector2 vectImp = impulse * (hitPoint - pos);
fix.body.applyLinearImpulse(ref vectImp, ref hitPoint);
if (!exploded.ContainsKey(fix))
{
exploded.Add(fix, vectImp);
}
}
return(exploded);
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
internal void Solve3()
{
Vector2 w1 = V[0].W;
Vector2 w2 = V[1].W;
Vector2 w3 = V[2].W;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
Vector2 e12 = w2 - w1;
float w1e12 = Vector2.Dot(w1, e12);
float w2e12 = Vector2.Dot(w2, e12);
float d12_1 = w2e12;
float d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
Vector2 e13 = w3 - w1;
float w1e13 = Vector2.Dot(w1, e13);
float w3e13 = Vector2.Dot(w3, e13);
float d13_1 = w3e13;
float d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
Vector2 e23 = w3 - w2;
float w2e23 = Vector2.Dot(w2, e23);
float w3e23 = Vector2.Dot(w3, e23);
float d23_1 = w3e23;
float d23_2 = -w2e23;
// Triangle123
float n123 = MathUtils.Cross(e12, e13);
float d123_1 = n123 * MathUtils.Cross(w2, w3);
float d123_2 = n123 * MathUtils.Cross(w3, w1);
float d123_3 = n123 * MathUtils.Cross(w1, w2);
// w1 playerRegion
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
SimplexVertex v0_1 = V[0];
v0_1.A = 1.0f;
V[0] = v0_1;
Count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
float inv_d12 = 1.0f / (d12_1 + d12_2);
SimplexVertex v0_2 = V[0];
SimplexVertex v1_2 = V[1];
v0_2.A = d12_1 * inv_d12;
v1_2.A = d12_2 * inv_d12;
V[0] = v0_2;
V[1] = v1_2;
Count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
float inv_d13 = 1.0f / (d13_1 + d13_2);
SimplexVertex v0_3 = V[0];
SimplexVertex v2_3 = V[2];
v0_3.A = d13_1 * inv_d13;
v2_3.A = d13_2 * inv_d13;
V[0] = v0_3;
V[2] = v2_3;
Count = 2;
V[1] = V[2];
return;
}
// w2 playerRegion
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
SimplexVertex v1_4 = V[1];
v1_4.A = 1.0f;
V[1] = v1_4;
Count = 1;
V[0] = V[1];
return;
}
// w3 playerRegion
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
SimplexVertex v2_5 = V[2];
v2_5.A = 1.0f;
V[2] = v2_5;
Count = 1;
V[0] = V[2];
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
float inv_d23 = 1.0f / (d23_1 + d23_2);
SimplexVertex v1_6 = V[1];
SimplexVertex v2_6 = V[2];
v1_6.A = d23_1 * inv_d23;
v2_6.A = d23_2 * inv_d23;
V[1] = v1_6;
V[2] = v2_6;
Count = 2;
V[0] = V[2];
return;
}
// Must be in triangle123
float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
SimplexVertex v0_7 = V[0];
SimplexVertex v1_7 = V[1];
SimplexVertex v2_7 = V[2];
v0_7.A = d123_1 * inv_d123;
v1_7.A = d123_2 * inv_d123;
v2_7.A = d123_3 * inv_d123;
V[0] = v0_7;
V[1] = v1_7;
V[2] = v2_7;
Count = 3;
}
//float noise(Vector2 n)
//{
// var d = new Vector2(0.0f, 1.0f);
// var b = Vector2.Floor(n);
// var f = Vector2.s(vec2(0.0), vec2(1.0), fract(n));
// return mix(mix(rand(b), rand(b + d.yx), f.x), mix(rand(b + d.xy), rand(b + d.yy), f.x), f.y);
//}
//float noise(float x, float y)
//{
//// 注目する点を囲む格子の頂点の値
// float v00 = value((int)x, (int)y);
// float v10 = value((int)x + 1, (int)y);
// float v01 = value((int)x, (int)y + 1);
// float v11 = value((int)x + 1, (int)y + 1);
// float tx = x - (int)x; // 入力から整数部を引いて少数部を取り出す
// float ty = y - (int)y;
// tx = Interpolate(tx); // 滑らかになるように曲線に変換
// ty = Interpolate(ty);
// float v0010 = Mix(v00, v10, tx);
// float v0111 = Mix(v01, v11, tx);
// return Mix(v0010, v0111, ty);
//}
float Random(Vector2 st)
{
return(Frac(Mathf.Sin(Vector2.Dot(st, new Vector2(12.9898f, 78.233f))) * 43758.5453123f));
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
LocalCenterA = b1.LocalCenter;
LocalCenterB = b2.LocalCenter;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
InvMassA = b1.InvMass;
InvIA = b1.InvI;
InvMassB = b2.InvMass;
InvIB = b2.InvI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if (_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
b1.LinearVelocityInternal -= InvMassA * P;
b1.AngularVelocityInternal -= InvIA * L1;
b2.LinearVelocityInternal += InvMassB * P;
b2.AngularVelocityInternal += InvIB * L2;
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
public static float Angle(Vector2 from, Vector2 to)
{
Normalize(ref from);
Normalize(ref to);
return(Mathf.Acos(Mathf.Clamp(Vector2.Dot(from, to), -1f, 1f)) * Mathf.Rad2Deg);
}
internal override bool SolvePositionConstraints()
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 c1 = b1.Sweep.C;
float a1 = b1.Sweep.A;
Vector2 c2 = b2.Sweep.C;
float a2 = b2.Sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
float angularError = Math.Abs(C1.Y);
if (active)
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C); // negated above
}
else
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c1 -= InvMassA * P;
a1 -= InvIA * L1;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b1.Sweep.C = c1;
b1.Sweep.A = a1;
b2.Sweep.C = c2;
b2.Sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// You cannot create a prismatic joint between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
Transform xf1 = b1.GetTransform();
Transform xf2 = b2.GetTransform();
// Compute the effective masses.
Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = xf1.TransformDirection(_localXAxis1);
_a1 = (d + r1).Cross(_axis);
_a2 = r2.Cross(_axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = xf1.TransformDirection(_localYAxis1);
_s1 = (d + r1).Cross(_perp);
_s2 = r2.Cross(_perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitState = LimitState.AtLowerLimit;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitState = LimitState.AtUpperLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
float L1 = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
float L2 = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
// Update is called once per frame
void FixedUpdate()
{
// First we just translate based on our velocity
transform.position += (Vector3)velocity * Time.fixedDeltaTime;
// Set the rotation
float angle = Mathf.Atan2(velocity.y, velocity.x) * Mathf.Rad2Deg;
transform.rotation = Quaternion.Lerp(transform.rotation, Quaternion.AngleAxis(angle, Vector3.forward), .05f);
// Add our acceleration
velocity += acceleration * Time.fixedDeltaTime;
if (velocity.magnitude > maxVelocity)
{
velocity = velocity.normalized * maxVelocity;
}
// Now we calculate the force towards the center of the flock
Vector2 cohesion;
Vector2 direction = manager.Center(group) - (Vector2)transform.position;
float proximity = direction.magnitude;
cohesion = direction.normalized * cohesionForce;
// Next we calculate the repulsion force (if any)
Vector2 repulsion = Vector2.zero;
foreach (GameObject other in others)
{
// The force is away from the other, and increases when closer to this
direction = (Vector2)transform.position - (Vector2)other.transform.position;
if (direction.magnitude < separationRange)
{
proximity = (separationRange - direction.magnitude) / separationRange;
proximity = Mathf.Clamp01(proximity);
proximity = proximity * proximity;
repulsion += direction.normalized * proximity * separationForce;
}
}
// Now we do some Object Avoidance stuff
// First, check if we are doing cone checking
Vector2 coneAvoidance = Vector2.zero;
if (coneCheck)
{
float closest = coneRange;
foreach (GameObject obstacle in obstacles)
{
direction = (Vector2)transform.position - (Vector2)obstacle.transform.position;
// check if the obstacle is in the cone
if (direction.magnitude < coneRange && direction.magnitude < closest && Vector2.Angle(velocity.normalized, direction.normalized * -1) < coneAngle)
{
coneAvoidance = direction.normalized * Mathf.Pow(((coneRange - direction.magnitude) / coneRange), 2);
closest = direction.magnitude;
}
}
}
// Now we do some collision prediction
Vector2 collisionAvoidance = Vector2.zero;
if (collisionPrediction)
{
// Only use the one that we will collide with the soonest
float shortestTime = 1;
// Check all obstacles inside our triggered area
foreach (GameObject obstacle in obstacles)
{
Vector2 relativePos = obstacle.transform.position - transform.position;
Vector2 relativeVel = obstacle.transform.GetComponentInParent <FlockingAvoidance>().velocity - velocity;
float t = -1 * ((Vector2.Dot(relativePos, relativeVel)) / Mathf.Pow(relativeVel.magnitude, 2));
float minSep = relativePos.magnitude - (relativeVel.magnitude * t);
if (t > 0 && t < shortestTime && minSep < .2f)
{
Vector2 targetPos = (Vector2)transform.position + velocity * t;
transform.GetChild(2).gameObject.SetActive(true);
transform.GetChild(2).position = (Vector3)targetPos;
shortestTime = t;
if (minSep <= 0 || relativePos.magnitude < .2f)
{
collisionAvoidance = relativePos.normalized * -1;
}
else
{
direction = (Vector2)transform.position - targetPos;
collisionAvoidance = direction.normalized;
}
}
}
}
if (collisionAvoidance == Vector2.zero)
{
transform.GetChild(2).gameObject.SetActive(false);
}
// If both avoidances are checked, we should average the two (otherwise they dont move
Vector2 avoidance = collisionAvoidance + coneAvoidance;
if (coneCheck && collisionPrediction)
{
avoidance /= 2f;
}
avoidance *= avoidanceForce;
//Finally we get the direction we should be heading
Vector2 align = manager.Direction(group) * alignForce;
// Sum up and clamp the accelerations
acceleration = cohesion + repulsion + align + avoidance;
if (acceleration.magnitude > maxAcceleration)
{
acceleration = acceleration.normalized * maxAcceleration;
}
velocity += acceleration * Time.fixedDeltaTime;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 c1 = b1._sweep.C;
float a1 = b1._sweep.A;
Vector2 c2 = b2._sweep.C;
float a2 = b2._sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
Vector2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
Vector2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = Box2DX.Common.Math.Mul(R1, _localXAxis1);
_a1 = (d + r1).Cross(_axis);
_a2 = r2.Cross(_axis);
float translation = Vector2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Box2DX.Common.Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = Box2DX.Common.Math.Mul(R1, _localYAxis1);
_s1 = (d + r1).Cross(_perp);
_s2 = r2.Cross(_perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(_perp, d);
C1.Y = a2 - a1 - _refAngle;
linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.X));
angularError = Box2DX.Common.Math.Abs(C1.Y);
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector3(k11, k12, k13);
_K.Col2 = new Vector3(k12, k22, k23);
_K.Col3 = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = _K.Solve33(-C);
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.C = c1;
b1._sweep.A = a1;
b2._sweep.C = c2;
b2._sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Process the input for this ship, from the gamepad assigned to it.
/// </summary>
/// <param name="elapsedTime">The amount of elapsed time, in seconds.</param>
/// <para
public virtual void ProcessInput(float elapsedTime, bool overlayPresent)
{
currentGamePadState = GamePad.GetState(playerIndex);
if (overlayPresent == false)
{
if (playing == false)
{
// trying to join - update the a-button timer
if (currentGamePadState.Buttons.A == ButtonState.Pressed)
{
aButtonTimer += elapsedTime;
}
else
{
aButtonTimer = 0f;
}
// if the timer has exceeded the expected value, join the game
if (aButtonTimer > aButtonHeldToPlay)
{
JoinGame();
}
}
else
{
// check if we're trying to leave
if (currentGamePadState.Buttons.B == ButtonState.Pressed)
{
bButtonTimer += elapsedTime;
}
else
{
bButtonTimer = 0f;
}
// if the timer has exceeded the expected value, leave the game
if (bButtonTimer > bButtonHeldToLeave)
{
LeaveGame();
}
else if (dead == false)
{
//
// the ship is alive, so process movement and firing
//
// calculate the current forward vector
Vector2 forward = new Vector2((float)Math.Sin(Rotation),
-(float)Math.Cos(Rotation));
Vector2 right = new Vector2(-forward.Y, forward.X);
// calculate the current left stick value
Vector2 leftStick = currentGamePadState.ThumbSticks.Left;
leftStick.Y *= -1f;
if (leftStick.LengthSquared() > 0f)
{
Vector2 wantedForward = Vector2.Normalize(leftStick);
float angleDiff = (float)Math.Acos(
Vector2.Dot(wantedForward, forward));
float facing = (Vector2.Dot(wantedForward, right) > 0f) ?
1f : -1f;
if (angleDiff > 0f)
{
Rotation += Math.Min(angleDiff, facing * elapsedTime *
rotationRadiansPerSecond);
}
// add velocity
Velocity += leftStick * (elapsedTime * speed);
if (Velocity.Length() > velocityLengthMaximum)
{
Velocity = Vector2.Normalize(Velocity) *
velocityLengthMaximum;
}
}
// check for firing with the right stick
Vector2 rightStick = currentGamePadState.ThumbSticks.Right;
rightStick.Y *= -1f;
if (rightStick.LengthSquared() > fireThresholdSquared)
{
weapon.Fire(Vector2.Normalize(rightStick));
}
// check for laying mines
if ((currentGamePadState.Buttons.B == ButtonState.Pressed) &&
(lastGamePadState.Buttons.B == ButtonState.Released))
{
// fire behind the ship
mineWeapon.Fire(-forward);
}
}
}
}
// update the gamepad state
lastGamePadState = currentGamePadState;
return;
}
public override void UpdateAccessory(Player player, bool hideVisual)
{
if (!FargowiltasSoulsDLC.Instance.CalamityLoaded)
{
return;
}
if (SoulConfig.Instance.GetValue(SoulConfig.Instance.calamityToggles.GodSlayerEffects))
{
calamity.Call("SetSetBonus", player, "godslayer", true);
calamity.Call("SetSetBonus", player, "godslayer_melee", true);
calamity.Call("SetSetBonus", player, "godslayer_ranged", true);
calamity.Call("SetSetBonus", player, "godslayer_magic", true);
calamity.Call("SetSetBonus", player, "godslayer_rogue", true);
}
if (SoulConfig.Instance.GetValue(SoulConfig.Instance.calamityToggles.MechwormMinion))
{
//summon
calamity.Call("SetSetBonus", player, "godslayer_summon", true);
if (player.whoAmI == Main.myPlayer)
{
if (player.FindBuffIndex(calamity.BuffType("Mechworm")) == -1)
{
player.AddBuff(calamity.BuffType("Mechworm"), 3600, true);
}
if (player.ownedProjectileCounts[calamity.ProjectileType("MechwormHead")] < 1)
{
int whoAmI = player.whoAmI;
int num = calamity.ProjectileType("MechwormHead");
int num2 = calamity.ProjectileType("MechwormBody");
int num3 = calamity.ProjectileType("MechwormBody2");
int num4 = calamity.ProjectileType("MechwormTail");
for (int i = 0; i < 1000; i++)
{
if (Main.projectile[i].active && Main.projectile[i].owner == whoAmI && (Main.projectile[i].type == num || Main.projectile[i].type == num4 || Main.projectile[i].type == num2 || Main.projectile[i].type == num3))
{
Main.projectile[i].Kill();
}
}
int num5 = player.maxMinions;
if (num5 > 10)
{
num5 = 10;
}
int num6 = (int)(35f * (player.minionDamage * 5f / 3f + player.minionDamage * 0.46f * (num5 - 1)));
Vector2 value = player.RotatedRelativePoint(player.MountedCenter, true);
Vector2 value2 = Utils.RotatedBy(Vector2.UnitX, player.fullRotation, default(Vector2));
Vector2 value3 = Main.MouseWorld - value;
float num7 = Main.mouseX + Main.screenPosition.X - value.X;
float num8 = Main.mouseY + Main.screenPosition.Y - value.Y;
if (player.gravDir == -1f)
{
num8 = Main.screenPosition.Y + Main.screenHeight - Main.mouseY - value.Y;
}
float num9 = (float)Math.Sqrt((num7 * num7 + num8 * num8));
if ((float.IsNaN(num7) && float.IsNaN(num8)) || (num7 == 0f && num8 == 0f))
{
num7 = player.direction;
num8 = 0f;
num9 = 10f;
}
else
{
num9 = 10f / num9;
}
num7 *= num9;
num8 *= num9;
int num10 = -1;
int num11 = -1;
for (int j = 0; j < 1000; j++)
{
if (Main.projectile[j].active && Main.projectile[j].owner == whoAmI)
{
if (num10 == -1 && Main.projectile[j].type == num)
{
num10 = j;
}
else if (num11 == -1 && Main.projectile[j].type == num4)
{
num11 = j;
}
if (num10 != -1 && num11 != -1)
{
break;
}
}
}
if (num10 == -1 && num11 == -1)
{
float num12 = Vector2.Dot(value2, value3);
if (num12 > 0f)
{
player.ChangeDir(1);
}
else
{
player.ChangeDir(-1);
}
num7 = 0f;
num8 = 0f;
value.X = Main.mouseX + Main.screenPosition.X;
value.Y = Main.mouseY + Main.screenPosition.Y;
int num13 = Projectile.NewProjectile(value.X, value.Y, num7, num8, calamity.ProjectileType("MechwormHead"), num6, 1f, whoAmI, 0f, 0f);
int num14 = num13;
num13 = Projectile.NewProjectile(value.X, value.Y, num7, num8, calamity.ProjectileType("MechwormBody"), num6, 1f, whoAmI, num14, 0f);
num14 = num13;
num13 = Projectile.NewProjectile(value.X, value.Y, num7, num8, calamity.ProjectileType("MechwormBody2"), num6, 1f, whoAmI, num14, 0f);
Main.projectile[num14].localAI[1] = num13;
Main.projectile[num14].netUpdate = true;
num14 = num13;
num13 = Projectile.NewProjectile(value.X, value.Y, num7, num8, calamity.ProjectileType("MechwormTail"), num6, 1f, whoAmI, num14, 0f);
Main.projectile[num14].localAI[1] = num13;
Main.projectile[num14].netUpdate = true;
}
else if (num10 != -1 && num11 != -1)
{
int num15 = Projectile.NewProjectile(value.X, value.Y, num7, num8, calamity.ProjectileType("MechwormBody"), num6, 1f, whoAmI, Main.projectile[num11].ai[0], 0f);
int num16 = Projectile.NewProjectile(value.X, value.Y, num7, num8, calamity.ProjectileType("MechwormBody2"), num6, 1f, whoAmI, num15, 0f);
Main.projectile[num15].localAI[1] = num16;
Main.projectile[num15].ai[1] = 1f;
Main.projectile[num15].minionSlots = 0f;
Main.projectile[num15].netUpdate = true;
Main.projectile[num16].localAI[1] = num11;
Main.projectile[num16].netUpdate = true;
Main.projectile[num16].minionSlots = 0f;
Main.projectile[num16].ai[1] = 1f;
Main.projectile[num11].ai[0] = num16;
Main.projectile[num11].netUpdate = true;
Main.projectile[num11].ai[1] = 1f;
}
}
}
}
if (SoulConfig.Instance.GetValue(SoulConfig.Instance.calamityToggles.NebulousCore))
{
calamity.GetItem("NebulousCore").UpdateAccessory(player, hideVisual);
}
//draedons heart
calamity.GetItem("DraedonsHeart").UpdateAccessory(player, hideVisual);
FargoDLCPlayer fargoPlayer = player.GetModPlayer <FargoDLCPlayer>();
fargoPlayer.GodSlayerEnchant = true;
fargoPlayer.AddPet(SoulConfig.Instance.calamityToggles.ChibiiPet, hideVisual, calamity.BuffType("ChibiiBuff"), calamity.ProjectileType("ChibiiDoggo"));
}
/// <summary>
/// Determines if a is perpendicular to b.
/// </summary>
/// <returns><c>true</c> if a is perpendicular to b, otherwise <c>false</c>.</returns>
/// <param name="a">The vector a.</param>
/// <param name="b">The vector b.</param>
public static bool IsPerp(Vector2 a, Vector2 b)
{
return(Equalsf(0.0f, Vector2.Dot(a, b)));
}
Vector4 QuadMoveTo(Vector3 WantPos, Vector3 Cur)
{
//Vertical Distance Prop Calculations
OS = DesiredHieght - Qtr.position.y - (Qrb.velocity.y / 2.0f);
AH += Time.deltaTime * OS * 0.5f;
Vector3 NoHB = new Vector3(Qtr.position.x, WantPos.y, Qtr.position.z);
float DH = DesiredHieght;
float throttle = (OS + AH) / Mathf.Max(0.1f, DesiredHieght);
//Node Type Handling
if (Vector3.Distance(WantPos, NoHB) < 0.2f && Qrb.velocity.magnitude < 0.5f && ttype == PointType.Waypoint)
{
ReachedTarget = true;
MH = 0;
return(new Vector4(throttle, 0, 0, 0));
}
else if (ttype == PointType.Pickup)
{
if (DesiredHieght > 0.25f && Qrb.velocity.y > -0.3f)
{
DesiredHieght -= Time.fixedDeltaTime;
}
else if (DesiredHieght <= 0.25f && Qrb.velocity.y > -0.1f && !GetComponentInChildren <UAVMagnet>().hasTrap)
{
DesiredHieght = 0.2f;
}
else if (GetComponentInChildren <UAVMagnet>().hasTrap)
{
DesiredHieght = 5;
ReachedTarget = true;
return(new Vector4(throttle, 0, 0, 0));
}
}
else if (Vector3.Distance(WantPos, NoHB) < .25f && Mathf.Abs(Qrb.velocity.y) < 0.8f && !ReachedTarget)
{
if (ttype == PointType.Dropoff)
{
if (DesiredHieght > 0.4f && Qrb.velocity.y > -0.3f)
{
DesiredHieght -= Time.fixedDeltaTime;
return(new Vector4(throttle, 0, 0, 0));
}
else if (DesiredHieght <= 0.4f && Qrb.velocity.y > -0.3f && Qtr.position.y <= 0.5f)
{
DesiredHieght = 5;
GetComponentInChildren <UAVMagnet>().Release();
}
else
{
return(new Vector4(throttle, 0, 0, 0));
}
}
else if (ttype == PointType.Takeoff)
{
DesiredHieght = 5;
}
else if (ttype == PointType.Land)
{
if (DesiredHieght > 0.1 && Qrb.velocity.y > -0.3f)
{
DesiredHieght -= Time.fixedDeltaTime;
return(new Vector4(throttle, 0, 0, 0));
}
else if (DesiredHieght <= 0.1f)
{
DesiredHieght = 0;
}
else
{
return(new Vector4(throttle, 0, 0, 0));
}
}
ReachedTarget = true;
return(new Vector4(throttle, 0, 0, 0));
}
if (DesiredHieght == 0)
{
return(Vector4.zero);
}
//Horizontal Distance Calculations
List <float> Dists = new List <float>();
Dists.Add(Vector3.Distance(P1.transform.position, WantPos));
Dists.Add(Vector3.Distance(P2.transform.position, WantPos));
Dists.Add(Vector3.Distance(P3.transform.position, WantPos));
Dists.Add(Vector3.Distance(P4.transform.position, WantPos));
int low = 0;
float lowDist = Dists[0];
Vector2 NV = new Vector2(Qrb.velocity.x, Qrb.velocity.z).normalized;
Vector2 TD = new Vector2(WantPos.x - Qtr.position.x, WantPos.z - Qtr.position.z).normalized;
float dir = Vector2.Dot(NV, TD);
float off = dir * (new Vector2(Qrb.velocity.x, Qrb.velocity.z).magnitude);
off *= 2f;
float dv = Vector2.Distance(new Vector2(Qtr.position.x, Qtr.position.z), new Vector2(WantPos.x, WantPos.z)) - off;
float rDist = Vector2.Distance(Qtr.position, WantPos);
dv = Mathf.Clamp(dv, -1f, 1f);
MH += Time.deltaTime;
if (ReachedTarget || ttype != PointType.Waypoint)
{
MH = 0;
}
for (int i = 0; i < Dists.Count; i++)
{
if (Dists[i] < lowDist)
{
low = i;
lowDist = Dists[i];
}
}
for (int i = 0; i < Dists.Count; i++)
{
Dists[i] -= Mathf.Clamp(8 * dv * (1 - (Dists[i] / rDist)), -0.2f, 10f);
Dists[i] -= lowDist;
}
float dist = Vector3.Distance(WantPos, NoHB);
float mult = Mathf.Clamp(Mathf.Sqrt(dist / 10.0f), 5.0f, 100.0f);
mult /= 2;
/*
* if(high == 0) P1.SpinProp(MotorPower/mult);
* if(high == 1) P2.SpinProp(MotorPower/mult);
* if(high == 2) P3.SpinProp(MotorPower/mult);
* if(high == 3) P4.SpinProp(MotorPower/mult);
*/
P1.SpinProp((MotorPower / ((mult) / Dists[0])));
P2.SpinProp((MotorPower / ((mult) / Dists[1])));
P3.SpinProp((MotorPower / ((mult) / Dists[2])));
P4.SpinProp((MotorPower / ((mult) / Dists[3])));
return(new Vector4(throttle, 0, 0, 0));
}
/// <summary>
/// When we enter something...
/// </summary>
/// <param name="collision"></param>
private void OnImpact(RaycastHit2D hit)
{
Collider2D other = hit.collider;
//Check for collideable surface
if (((1 << other.gameObject.layer) & collisionMask) != 0)
{
//Check for one way platforms
if (!(other.CompareTag(PlatformerController.TAG_ONEWAYPLATFORM) && Vector2.Dot(velocity, Vector2.up) > .5f))
{
bool isEnemy = false;
//Check for hittable
IHittable hittable = other.gameObject.GetComponent <IHittable>();
//If we hit something and it is not player affiliated
if (hittable != null && hittable.GetTeam() != Team.Player)
{
//Determine if it is an enemy
isEnemy = hittable.GetTeam() == Team.Enemy;
//If it is not a block
if (!(slimeType == SlimeType.Purple && other.CompareTag("Block")))
{
hittable.OnHit(1, 2 * (other.transform.position - transform.position));
}
}
//Do something, depending on what type of slime we are
switch (slimeType)
{
case SlimeType.Green:
//Only do it on vertical faces
if (!isEnemy && Vector3.Dot(hit.normal, Vector3.up) > .9f)
{
//Make drop prefab
Instantiate(dropPrefab, hit.point - (Vector2.up * .25f), Quaternion.identity);
}
break;
case SlimeType.Purple:
if (!isEnemy)
{
//Round position
Vector3 pos = transform.position;
pos.x = Mathf.Round(pos.x / 2) * 2;
pos.y = Mathf.Round(pos.y / 2) * 2;
Instantiate(blockPrefab, pos, Quaternion.identity);
}
break;
case SlimeType.Gold:
//Make explosion prefab
Instantiate(explosionPrefab, hit.point, Quaternion.identity);
//Increase particle size
splashParticles.transform.localScale = Vector3.one * 1.5f;
break;
}
//Play particle effect
splashParticles.transform.SetParent(null);
splashParticles.Play();
audio.Play();
//Destroy self
Destroy(gameObject);
}
}
}
//Cutting a shape into two is based on the work of Daid and his prototype BoxCutter: http://www.box2d.org/forum/viewtopic.php?f=3&t=1473
/// <summary>
/// Split a fixture into 2 vertice collections using the given entry and exit-point.
/// </summary>
/// <param name="fixture">The Fixture to split</param>
/// <param name="entryPoint">The entry point - The start point</param>
/// <param name="exitPoint">The exit point - The end point</param>
/// <param name="first">The first collection of vertexes</param>
/// <param name="second">The second collection of vertexes</param>
public static void SplitShape(Fixture fixture, Vector2 entryPoint, Vector2 exitPoint, out Vertices first, out Vertices second)
{
Vector2 localEntryPoint = fixture.Body.GetLocalPoint(ref entryPoint);
Vector2 localExitPoint = fixture.Body.GetLocalPoint(ref exitPoint);
PolygonShape shape = fixture.Shape as PolygonShape;
//We can only cut polygons at the moment
if (shape == null)
{
first = new Vertices();
second = new Vertices();
return;
}
//Offset the entry and exit points if they are too close to the vertices
foreach (Vector2 vertex in shape.Vertices)
{
if (vertex.Equals(localEntryPoint))
{
localEntryPoint -= new Vector2(0, Settings.Epsilon);
}
if (vertex.Equals(localExitPoint))
{
localExitPoint += new Vector2(0, Settings.Epsilon);
}
}
Vertices vertices = new Vertices(shape.Vertices);
Vertices[] newPolygon = new Vertices[2];
for (int i = 0; i < newPolygon.Length; i++)
{
newPolygon[i] = new Vertices(vertices.Count);
}
int[] cutAdded = { -1, -1 };
int last = -1;
for (int i = 0; i < vertices.Count; i++)
{
int n;
//Find out if this vertex is on the old or new shape.
if (Vector2.Dot(MathUtils.Cross(localExitPoint - localEntryPoint, 1), vertices[i] - localEntryPoint) > Settings.Epsilon)
{
n = 0;
}
else
{
n = 1;
}
if (last != n)
{
//If we switch from one shape to the other add the cut vertices.
if (last == 0)
{
Debug.Assert(cutAdded[0] == -1);
cutAdded[0] = newPolygon[last].Count;
newPolygon[last].Add(localExitPoint);
newPolygon[last].Add(localEntryPoint);
}
if (last == 1)
{
Debug.Assert(cutAdded[last] == -1);
cutAdded[last] = newPolygon[last].Count;
newPolygon[last].Add(localEntryPoint);
newPolygon[last].Add(localExitPoint);
}
}
newPolygon[n].Add(vertices[i]);
last = n;
}
//Add the cut in case it has not been added yet.
if (cutAdded[0] == -1)
{
cutAdded[0] = newPolygon[0].Count;
newPolygon[0].Add(localExitPoint);
newPolygon[0].Add(localEntryPoint);
}
if (cutAdded[1] == -1)
{
cutAdded[1] = newPolygon[1].Count;
newPolygon[1].Add(localEntryPoint);
newPolygon[1].Add(localExitPoint);
}
for (int n = 0; n < 2; n++)
{
Vector2 offset;
if (cutAdded[n] > 0)
{
offset = (newPolygon[n][cutAdded[n] - 1] - newPolygon[n][cutAdded[n]]);
}
else
{
offset = (newPolygon[n][newPolygon[n].Count - 1] - newPolygon[n][0]);
}
offset.Normalize();
if (!offset.IsValid())
{
offset = Vector2.One;
}
newPolygon[n][cutAdded[n]] += Settings.Epsilon * offset;
if (cutAdded[n] < newPolygon[n].Count - 2)
{
offset = (newPolygon[n][cutAdded[n] + 2] - newPolygon[n][cutAdded[n] + 1]);
}
else
{
offset = (newPolygon[n][0] - newPolygon[n][newPolygon[n].Count - 1]);
}
offset.Normalize();
if (!offset.IsValid())
{
offset = Vector2.One;
}
newPolygon[n][cutAdded[n] + 1] += Settings.Epsilon * offset;
}
first = newPolygon[0];
second = newPolygon[1];
}
private void CheckFrozenEdge(
Vector2 origin, Vector2 neighbor,
Vector2 pos, Vector2 posNext, Vector2 velocity, ref List<CollisionSubframeBuffer> collisionBuffer,
CollisionSubframeBuffer subframeToAdd
)
{
Vector2 intersection = new Vector2();
if (CollideSweptSegments(new LineSegment(origin, neighbor), new LineSegment(pos, posNext), ref intersection))
{
double particleOffset = (intersection - pos).Length();
if (particleOffset > epsilon) // to prevent slipping of start point to just reflected edge // TODO: check if this condition is really usefull. may be (timeOffset > 0.0) is a sufficient condition
{
// reflect velocity relative edge
Vector2 reflectSurface = neighbor - origin;
Vector2 reflectNormal = new Vector2(-reflectSurface.Y, reflectSurface.X);
if (reflectNormal.Dot(velocity) < 0) reflectNormal = -reflectNormal;
Vector2 newVelocity = velocity - (1.0 + coefficientElasticity) * reflectNormal * (reflectNormal.Dot(velocity) / reflectNormal.LengthSq());
subframeToAdd.vParticle = newVelocity;
subframeToAdd.vEdgeStart = new Vector2(0.0, 0.0);
subframeToAdd.vEdgeEnd = new Vector2(0.0, 0.0);
subframeToAdd.timeCoefficient = particleOffset / velocity.Length();
collisionBuffer.Add(subframeToAdd);
}
}
}
void CalculatePassengerMovement(Vector3 velocity)
{
Dictionary <Transform, IPlatformMoveBlocker> movedPassengers = new Dictionary <Transform, IPlatformMoveBlocker> ();
passengerMovement = new List <PassengerMovement> ();
System.Func <Vector2, RaycastHit2D, PassengerMovement> GetMovement = (dir, hit) => {
IPlatformMoveBlocker blocker = movedPassengers[hit.transform];
// add 90 to convert from Vector2.down being default direction to Vector2.right
Vector2 gravityAngle = Utils.AngleToVector(blocker.GravityAngle - 90);
Vector2 normalizedVelocity = velocity.normalized;
var rotatedVelocity = velocity.Rotate(-blocker.GravityAngle);
float dot = Vector2.Dot(gravityAngle, normalizedVelocity);
bool movingWithDirection = rotatedVelocity.y > 0;
// we need to check the direction cast in comparison to gravity to determine
// players position relative to the platform (i.e. player on top)
float positionDot = Vector2.Dot(gravityAngle, dir);
bool onTop = positionDot < -0.9f;
float velocityDot = Vector2.Dot(normalizedVelocity, dir);
if (dot > 0.5f)
{
// gravity and platform moving in same direction
if (onTop)
{
// passenger is on top of the platform and should be moved with it
float pushX = velocity.x;
float pushY = velocity.y;
return(new PassengerMovement(hit.transform, new Vector3(pushX, pushY), true, false, false));
}
else
{
// passenger is below the platform and will be pushed
float pushX = 0f;
float pushY = rotatedVelocity.y - (hit.distance - skinWidth) * Mathf.Sign(rotatedVelocity.y);
return(new PassengerMovement(hit.transform, new Vector3(pushX, pushY), false, true, true));
}
}
else if (dot < -0.5f)
{
// gravity and platform moving in opposite directions
float pushX = movingWithDirection ? rotatedVelocity.x : 0;
float pushY = rotatedVelocity.y - (hit.distance - skinWidth) * Mathf.Sign(rotatedVelocity.y);
return(new PassengerMovement(hit.transform, new Vector3(pushX, pushY), onTop, true, false));
}
else if (positionDot < 0.9f)
{
// platform is moving side-to-side relative to gravity
// only disallow movement is player is below platform
float pushX = rotatedVelocity.x - (hit.distance - skinWidth) * Mathf.Sign(rotatedVelocity.x);
float pushY = 0f;
return(new PassengerMovement(hit.transform, new Vector3(pushX, pushY), onTop, false, false));
}
return(null);
};
System.Action <Vector2, Vector2, float> CastForPoint = (pt, dir, dist) => {
//Debug.DrawRay(pt, dir*dist, new Color(dir.y*0.5f+0.5f, dir.x*0.5f+0.5f, 0), 0.1f);
RaycastHit2D hit = Physics2D.Raycast(pt, dir, dist, passengerMask);
if (hit)
{
if (!movedPassengers.ContainsKey(hit.transform))
{
movedPassengers.Add(hit.transform, hit.transform.GetComponent <IPlatformMoveBlocker>());
PassengerMovement m = GetMovement(dir, hit);
if (m != null)
{
passengerMovement.Add(m);
}
}
else
{
PassengerMovement m = GetMovement(dir, hit);
int index = passengerMovement.FindIndex(pm => pm.transform == hit.transform);
if (m != null && index >= 0 && m.velocity.sqrMagnitude > passengerMovement[index].velocity.sqrMagnitude)
{
passengerMovement[index] = m;
}
}
}
};
var pts = GeneratePoints();
float topDistance = skinWidth * 2f;
float bottomDistance = skinWidth * 2f;
Vector2 rotatedPlatformVelocity = velocity.Rotate(-transform.eulerAngles.z);
if (rotatedPlatformVelocity.y > 0)
{
topDistance = Mathf.Max(topDistance, Mathf.Abs(rotatedPlatformVelocity.y) + skinWidth);
}
else
{
bottomDistance = Mathf.Max(bottomDistance, Mathf.Abs(rotatedPlatformVelocity.y) + skinWidth);
}
Vector2 up = Vector2.up.Rotate(transform.eulerAngles.z);
Vector2 right = Vector2.right.Rotate(transform.eulerAngles.z);
foreach (var pt in pts.Top)
{
CastForPoint(pt, up, topDistance);
}
foreach (var pt in pts.Bottom)
{
CastForPoint(pt, -up, bottomDistance);
}
float leftDistance = skinWidth * 2f;
float rightDistance = skinWidth * 2f;
if (rotatedPlatformVelocity.x > 0)
{
rightDistance = Mathf.Max(rightDistance, Mathf.Abs(rotatedPlatformVelocity.x) + skinWidth);
}
else
{
leftDistance = Mathf.Max(leftDistance, Mathf.Abs(rotatedPlatformVelocity.x) + skinWidth);
}
foreach (var pt in pts.Right)
{
CastForPoint(pt, right, rightDistance);
}
foreach (var pt in pts.Left)
{
CastForPoint(pt, -right, leftDistance);
}
}
public static double Dot(Vector2 a, Vector2 b)
{
return a.Dot(b);
}
private static void SolveManifold(Manifold Manifold)
{
// Early out and positional correct if both objects have infinite mass
if (Manifold.BodyA.Mass + Manifold.BodyB.Mass == 0)
{
Manifold.BodyA.Velocity = Vector2.Zero;
Manifold.BodyB.Velocity = Vector2.Zero;
return;
}
for (uint i = 0; i < Manifold.Points.Length; ++i)
{
// Calculate radii from COM to contact
Vector2 rA = Manifold.Points[i] - Manifold.BodyA.Position;
Vector2 rB = Manifold.Points[i] - Manifold.BodyB.Position;
// Relative velocity
Vector2 rv = Manifold.BodyB.Velocity + (rB * Manifold.BodyB.AngularVelocity) - Manifold.BodyA.Velocity - (rA * Manifold.BodyA.AngularVelocity);
// Relative velocity along the normal
Number contactVel = rv.Dot(Manifold.Normal);
// Do not resolve if velocities are separating
if (contactVel > 0)
{
return;
}
Number invMassA = (Manifold.BodyA.Mass == 0 ? (Number)0 : 1 / Manifold.BodyA.Mass);
Number invMassB = (Manifold.BodyB.Mass == 0 ? (Number)0 : 1 / Manifold.BodyB.Mass);
Number invInertiaA = (Manifold.BodyA.Inertia == 0 ? (Number)0 : 1 / Manifold.BodyA.Inertia);
Number invInertiaB = (Manifold.BodyB.Inertia == 0 ? (Number)0 : 1 / Manifold.BodyB.Inertia);
Number rACrossNormal = rA.Cross(Manifold.Normal);
Number rBCrossNormal = rB.Cross(Manifold.Normal);
Number invMassSum = (rACrossNormal * rACrossNormal) * invInertiaA + (rBCrossNormal * rBCrossNormal) * invInertiaB + invMassA + invMassB;
invMassSum = Math.Max(1, invMassSum);
// Calculate impulse scalar
Number j = -(1.0F + Manifold.MixedRestitution) * contactVel;
j /= invMassSum;
j /= Manifold.Points.Length;
// Apply impulse
Vector2 impulse = Manifold.Normal * j;
ApplyImpulse(Manifold.BodyA, -impulse, rA);
ApplyImpulse(Manifold.BodyB, impulse, rB);
// Friction impulse
rv = Manifold.BodyB.Velocity + (rB * Manifold.BodyB.AngularVelocity) - Manifold.BodyA.Velocity - (rA * Manifold.BodyA.AngularVelocity);
Vector2 t = rv - (Manifold.Normal * rv.Dot(Manifold.Normal));
t.Normalize();
// j tangent magnitude
Number jt = -rv.Dot(t);
jt /= invMassSum;
jt /= (Number)Manifold.Points.Length;
// Don't apply tiny friction impulses
if (jt == 0)
{
return;
}
// Coulumb's law
Vector2 tangentImpulse;
if (Math.Abs(jt) < j * Manifold.MixedStaticFriction)
{
tangentImpulse = t * jt;
}
else
{
tangentImpulse = t * -j * Manifold.MixedDynamicFriction;
}
// Apply friction impulse
ApplyImpulse(Manifold.BodyA, -tangentImpulse, rA);
ApplyImpulse(Manifold.BodyB, tangentImpulse, rB);
}
}
