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