private static FP BasinAngle(AdvancingFrontNode node) { FP x = node.Point.X - node.Next.Next.Point.X; FP y = node.Point.Y - node.Next.Next.Point.Y; return(FP.Atan2(y, x)); }
private bool AngleSign() { Point point = this._head.Next - this._head; Point p = this._tail - this._head; return(FP.Atan2(point.Cross(p), point.Dot(p)) >= 0); }
private FP Angle(Point p) { Point point = p.Next - p; Point p2 = p.Prev - p; return(FP.Atan2(point.Cross(p2), point.Dot(p2))); }
public List <FPVector> SubdivideEvenly(int divisions) { List <FPVector> verts = new List <FPVector>(); FP length = GetLength(); FP deltaLength = length / divisions + 0.001f; FP t = 0.000f; // we always start at the first control point FPVector2 start = ControlPoints[0]; FPVector2 end = GetPosition(t); // increment t until we are at half the distance while (deltaLength * 0.5f >= FPVector2.Distance(start, end)) { end = GetPosition(t); t += 0.0001f; if (t >= 1f) { break; } } start = end; // for each box for (int i = 1; i < divisions; i++) { FPVector2 normal = GetPositionNormal(t); FP angle = FP.Atan2(normal.y, normal.x); verts.Add(new FPVector(end.x, end.y, angle)); // until we reach the correct distance down the curve while (deltaLength >= FPVector2.Distance(start, end)) { end = GetPosition(t); t += 0.00001f; if (t >= 1f) { break; } } if (t >= 1f) { break; } start = end; } return(verts); }
private static FP HoleAngle(AdvancingFrontNode node) { FP x = node.Point.X; FP y = node.Point.Y; FP x2 = node.Next.Point.X - x; FP x3 = node.Next.Point.Y - y; FP y2 = node.Prev.Point.X - x; FP y3 = node.Prev.Point.Y - y; return(FP.Atan2(x2 * y3 - x3 * y2, x2 * y2 + x3 * y3)); }
private static FP Angle(TriangulationPoint origin, TriangulationPoint pa, TriangulationPoint pb) { FP x = origin.X; FP y = origin.Y; FP x2 = pa.X - x; FP x3 = pa.Y - y; FP y2 = pb.X - x; FP y3 = pb.Y - y; FP y4 = x2 * y3 - x3 * y2; FP x4 = x2 * y2 + x3 * y3; return(FP.Atan2(y4, x4)); }
/// <summary> /// Called once before iteration starts. /// </summary> /// <param name="timestep">The 5simulation timestep</param> public override void PrepareForIteration(FP timestep) { effectiveMass = body1.invInertiaWorld + body2.invInertiaWorld; softnessOverDt = softness / timestep; effectiveMass.M11 += softnessOverDt; effectiveMass.M22 += softnessOverDt; effectiveMass.M33 += softnessOverDt; TSMatrix.Inverse(ref effectiveMass, out effectiveMass); TSMatrix orientationDifference; TSMatrix.Multiply(ref initialOrientation1, ref initialOrientation2, out orientationDifference); TSMatrix.Transpose(ref orientationDifference, out orientationDifference); TSMatrix q = orientationDifference * body2.invOrientation * body1.orientation; TSVector axis; FP x = q.M32 - q.M23; FP y = q.M13 - q.M31; FP z = q.M21 - q.M12; FP r = TSMath.Sqrt(x * x + y * y + z * z); FP t = q.M11 + q.M22 + q.M33; FP angle = FP.Atan2(r, t - 1); axis = new TSVector(x, y, z) * angle; if (r != FP.Zero) { axis = axis * (FP.One / r); } bias = axis * biasFactor * (-FP.One / timestep); // Apply previous frame solution as initial guess for satisfying the constraint. if (!body1.IsStatic) { body1.angularVelocity += TSVector.Transform(accumulatedImpulse, body1.invInertiaWorld); } if (!body2.IsStatic) { body2.angularVelocity += TSVector.Transform(-FP.One * accumulatedImpulse, body2.invInertiaWorld); } }
public static FP VectorAngle(ref TSVector2 p1, ref TSVector2 p2) { FP y = FP.Atan2(p1.y, p1.x); FP x = FP.Atan2(p2.y, p2.x); FP fP = x - y; while (fP > FP.Pi) { fP -= 2 * FP.Pi; } while (fP < -FP.Pi) { fP += 2 * FP.Pi; } return(fP); }
/// <summary> /// Return the angle between two vectors on a plane /// The angle is from vector 1 to vector 2, positive anticlockwise /// The result is between -pi -> pi /// </summary> public static FP VectorAngle(ref TSVector2 p1, ref TSVector2 p2) { FP theta1 = FP.Atan2(p1.y, p1.x); FP theta2 = FP.Atan2(p2.y, p2.x); FP dtheta = theta2 - theta1; while (dtheta > FP.Pi) { dtheta -= (2 * FP.Pi); } while (dtheta < -FP.Pi) { dtheta += (2 * FP.Pi); } return(dtheta); }
public List <Vector3> SubdivideEvenly(int divisions) { List <Vector3> list = new List <Vector3>(); FP length = this.GetLength(); FP x = length / divisions + 0.001f; FP fP = 0f; TSVector2 value = this.ControlPoints[0]; TSVector2 position = this.GetPosition(fP); while (x * 0.5f >= TSVector2.Distance(value, position)) { position = this.GetPosition(fP); fP += 0.0001f; bool flag = fP >= 1f; if (flag) { break; } } value = position; for (int i = 1; i < divisions; i++) { TSVector2 positionNormal = this.GetPositionNormal(fP); FP z = FP.Atan2(positionNormal.y, positionNormal.x); list.Add(new Vector3(position, z)); while (x >= TSVector2.Distance(value, position)) { position = this.GetPosition(fP); fP += 1E-05f; bool flag2 = fP >= 1f; if (flag2) { break; } } bool flag3 = fP >= 1f; if (flag3) { break; } value = position; } return(list); }
/// <summary> /// Get the angle in radians /// </summary> public FP GetAngle() { return(FP.Atan2(s, c)); }
/// <summary> /// Returns the arc tan of coordinates x-y. /// </summary> public static FP Atan2(FP y, FP x) { return(FP.Atan2(y, x)); }
/// <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, TSVector2> Activate(TSVector2 pos, FP radius, FP maxForce) { AABB aabb; aabb.LowerBound = pos + new TSVector2(-radius, -radius); aabb.UpperBound = pos + new TSVector2(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, TSVector2>()); } Dictionary <Fixture, TSVector2> exploded = new Dictionary <Fixture, TSVector2>(shapeCount + containedShapeCount); // Per shape max/min angles for now. FP[] vals = new FP[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(); TSVector2 vec = TSVector2.zero + new TSVector2(cs.Radius, 0); v.Add(vec); vec = TSVector2.zero + new TSVector2(0, cs.Radius); v.Add(vec); vec = TSVector2.zero + new TSVector2(-cs.Radius, cs.Radius); v.Add(vec); vec = TSVector2.zero + new TSVector2(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) { TSVector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MassData.Centroid) - pos; FP angleToCentroid = FP.Atan2(toCentroid.y, toCentroid.x); FP min = FP.MaxValue; FP max = FP.MinValue; FP minAbsolute = 0.0f; FP maxAbsolute = 0.0f; for (int j = 0; j < ps.Vertices.Count; ++j) { TSVector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos); FP newAngle = FP.Atan2(toVertex.y, toVertex.x); FP diff = (newAngle - angleToCentroid); diff = (diff - FP.Pi) % (2 * FP.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 * FP.Pi; // correction for not handling negs } diff -= FP.Pi; if (FP.Abs(diff) > FP.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; FP 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] + FP.PiTimes2 + vals[i]); } else { midpt = (vals[i + 1] + vals[i]); } midpt = midpt / 2; TSVector2 p1 = pos; TSVector2 p2 = radius * new TSVector2(FP.Cos(midpt), FP.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 -= FP.PiTimes2; _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 - FP.PiTimes2; _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; } FP arclen = _data[i].Max - _data[i].Min; FP first = TSMath.Min(MaxEdgeOffset, EdgeRatio * arclen); int insertedRays = FP.Ceiling((((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle)).AsInt(); if (insertedRays < 0) { insertedRays = 0; } FP offset = (arclen - first * 2.0f) / ((FP)MinRays + insertedRays - 1); //Note: This loop can go into infinite as it operates on FPs. //Added FPEquals with a large epsilon. for (FP j = _data[i].Min + first; j < _data[i].Max || MathUtils.FPEquals(j, _data[i].Max, 0.0001f); j += offset) { TSVector2 p1 = pos; TSVector2 p2 = pos + radius * new TSVector2(FP.Cos(j), FP.Sin(j)); TSVector2 hitpoint = TSVector2.zero; FP minlambda = FP.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. FP impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / FP.Pi * (1.0f - SyncFrame.TSMath.Min(FP.One, minlambda)); // We Apply the impulse!!! TSVector2 vectImp = TSVector2.Dot(impulse * new TSVector2(FP.Cos(j), FP.Sin(j)), -ro.Normal) * new TSVector2(FP.Cos(j), FP.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; } FP impulse = MinRays * maxForce * 180.0f / FP.Pi; TSVector2 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); } TSVector2 vectImp = impulse * (hitPoint - pos); fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint); if (!exploded.ContainsKey(fix)) { exploded.Add(fix, vectImp); } } return(exploded); }
public Dictionary <Fixture, TSVector2> Activate(TSVector2 pos, FP radius, FP maxForce) { AABB aABB; aABB.LowerBound = pos + new TSVector2(-radius, -radius); aABB.UpperBound = pos + new TSVector2(radius, radius); Fixture[] shapes = new Fixture[this.MaxShapes]; Fixture[] containedShapes = new Fixture[5]; bool exit = false; int shapeCount = 0; int containedShapeCount = 0; this.World.QueryAABB(delegate(Fixture fixture) { bool flag22 = fixture.TestPoint(ref pos); bool result2; if (flag22) { bool ignoreWhenInsideShape = this.IgnoreWhenInsideShape; if (ignoreWhenInsideShape) { exit = true; result2 = false; return(result2); } Fixture[] arg_45_0 = containedShapes; int num4 = containedShapeCount; containedShapeCount = num4 + 1; arg_45_0[num4] = fixture; } else { Fixture[] arg_62_0 = shapes; int num4 = shapeCount; shapeCount = num4 + 1; arg_62_0[num4] = fixture; } result2 = true; return(result2); }, ref aABB); bool exit2 = exit; Dictionary <Fixture, TSVector2> result; if (exit2) { result = new Dictionary <Fixture, TSVector2>(); } else { Dictionary <Fixture, TSVector2> dictionary = new Dictionary <Fixture, TSVector2>(shapeCount + containedShapeCount); FP[] array = new FP[shapeCount * 2]; int num = 0; for (int i = 0; i < shapeCount; i++) { CircleShape circleShape = shapes[i].Shape as CircleShape; bool flag = circleShape != null; PolygonShape polygonShape; if (flag) { Vertices vertices = new Vertices(); TSVector2 item = TSVector2.zero + new TSVector2(circleShape.Radius, 0); vertices.Add(item); item = TSVector2.zero + new TSVector2(0, circleShape.Radius); vertices.Add(item); item = TSVector2.zero + new TSVector2(-circleShape.Radius, circleShape.Radius); vertices.Add(item); item = TSVector2.zero + new TSVector2(0, -circleShape.Radius); vertices.Add(item); polygonShape = new PolygonShape(vertices, 0); } else { polygonShape = (shapes[i].Shape as PolygonShape); } bool flag2 = shapes[i].Body.BodyType == BodyType.Dynamic && polygonShape != null; if (flag2) { TSVector2 tSVector = shapes[i].Body.GetWorldPoint(polygonShape.MassData.Centroid) - pos; FP y = FP.Atan2(tSVector.y, tSVector.x); FP y2 = FP.MaxValue; FP y3 = FP.MinValue; FP fP = 0f; FP fP2 = 0f; for (int j = 0; j < polygonShape.Vertices.Count; j++) { TSVector2 tSVector2 = shapes[i].Body.GetWorldPoint(polygonShape.Vertices[j]) - pos; FP fP3 = FP.Atan2(tSVector2.y, tSVector2.x); FP fP4 = fP3 - y; fP4 = (fP4 - MathHelper.Pi) % (2 * MathHelper.Pi); bool flag3 = fP4 < 0f; if (flag3) { fP4 += 2 * MathHelper.Pi; } fP4 -= MathHelper.Pi; bool flag4 = FP.Abs(fP4) > MathHelper.Pi; if (!flag4) { bool flag5 = fP4 > y3; if (flag5) { y3 = fP4; fP2 = fP3; } bool flag6 = fP4 < y2; if (flag6) { y2 = fP4; fP = fP3; } } } array[num] = fP; num++; array[num] = fP2; num++; } } Array.Sort <FP>(array, 0, num, this._rdc); this._data.Clear(); bool flag7 = true; for (int k = 0; k < num; k++) { Fixture fixture = null; int num2 = (k == num - 1) ? 0 : (k + 1); bool flag8 = array[k] == array[num2]; if (!flag8) { bool flag9 = k == num - 1; FP x; if (flag9) { x = array[0] + MathHelper.Pi * 2 + array[k]; } else { x = array[k + 1] + array[k]; } x /= 2; TSVector2 pos2 = pos; TSVector2 point = radius * new TSVector2(FP.Cos(x), FP.Sin(x)) + pos; bool hitClosest = false; this.World.RayCast(delegate(Fixture f, TSVector2 p, TSVector2 n, FP fr) { Body body = f.Body; bool flag22 = !this.IsActiveOn(body); FP result2; if (flag22) { result2 = 0; } else { hitClosest = true; fixture = f; result2 = fr; } return(result2); }, pos2, point); bool flag10 = hitClosest && fixture.Body.BodyType == BodyType.Dynamic; if (flag10) { bool flag11 = this._data.Any <ShapeData>() && this._data.Last <ShapeData>().Body == fixture.Body && !flag7; if (flag11) { int index = this._data.Count - 1; ShapeData value = this._data[index]; value.Max = array[num2]; this._data[index] = value; } else { ShapeData item2; item2.Body = fixture.Body; item2.Min = array[k]; item2.Max = array[num2]; this._data.Add(item2); } bool flag12 = this._data.Count > 1 && k == num - 1 && this._data.Last <ShapeData>().Body == this._data.First <ShapeData>().Body&& this._data.Last <ShapeData>().Max == this._data.First <ShapeData>().Min; if (flag12) { ShapeData value2 = this._data[0]; value2.Min = this._data.Last <ShapeData>().Min; this._data.RemoveAt(this._data.Count - 1); this._data[0] = value2; while (this._data.First <ShapeData>().Min >= this._data.First <ShapeData>().Max) { value2.Min -= MathHelper.Pi * 2; this._data[0] = value2; } } int index2 = this._data.Count - 1; ShapeData value3 = this._data[index2]; while (this._data.Count > 0 && this._data.Last <ShapeData>().Min >= this._data.Last <ShapeData>().Max) { value3.Min = this._data.Last <ShapeData>().Min - 2 * MathHelper.Pi; this._data[index2] = value3; } flag7 = false; } else { flag7 = true; } } } for (int l = 0; l < this._data.Count; l++) { bool flag13 = !this.IsActiveOn(this._data[l].Body); if (!flag13) { FP fP5 = this._data[l].Max - this._data[l].Min; FP fP6 = MathHelper.Min(RealExplosion.MaxEdgeOffset, this.EdgeRatio * fP5); int num3 = FP.Ceiling((fP5 - 2f * fP6 - (this.MinRays - 1) * this.MaxAngle) / this.MaxAngle).AsInt(); bool flag14 = num3 < 0; if (flag14) { num3 = 0; } FP y4 = (fP5 - fP6 * 2f) / (this.MinRays + num3 - 1); FP fP7 = this._data[l].Min + fP6; while (fP7 < this._data[l].Max || MathUtils.FPEquals(fP7, this._data[l].Max, 0.0001f)) { TSVector2 pos3 = pos; TSVector2 tSVector3 = pos + radius * new TSVector2(FP.Cos(fP7), FP.Sin(fP7)); TSVector2 tSVector4 = TSVector2.zero; FP fP8 = FP.MaxValue; List <Fixture> fixtureList = this._data[l].Body.FixtureList; for (int m = 0; m < fixtureList.Count; m++) { Fixture fixture3 = fixtureList[m]; RayCastInput rayCastInput; rayCastInput.Point1 = pos3; rayCastInput.Point2 = tSVector3; rayCastInput.MaxFraction = 50f; RayCastOutput rayCastOutput; bool flag15 = fixture3.RayCast(out rayCastOutput, ref rayCastInput, 0); if (flag15) { bool flag16 = fP8 > rayCastOutput.Fraction; if (flag16) { fP8 = rayCastOutput.Fraction; tSVector4 = rayCastOutput.Fraction * tSVector3 + (1 - rayCastOutput.Fraction) * pos3; } } FP scaleFactor = fP5 / (this.MinRays + num3) * maxForce * 180f / MathHelper.Pi * (1f - TSMath.Min(FP.One, fP8)); TSVector2 tSVector5 = TSVector2.Dot(scaleFactor * new TSVector2(FP.Cos(fP7), FP.Sin(fP7)), -rayCastOutput.Normal) * new TSVector2(FP.Cos(fP7), FP.Sin(fP7)); this._data[l].Body.ApplyLinearImpulse(ref tSVector5, ref tSVector4); bool flag17 = dictionary.ContainsKey(fixture3); if (flag17) { Dictionary <Fixture, TSVector2> dictionary2 = dictionary; Fixture key = fixture3; dictionary2[key] += tSVector5; } else { dictionary.Add(fixture3, tSVector5); } bool flag18 = fP8 > 1f; if (flag18) { tSVector4 = tSVector3; } } fP7 += y4; } } } for (int n2 = 0; n2 < containedShapeCount; n2++) { Fixture fixture2 = containedShapes[n2]; bool flag19 = !this.IsActiveOn(fixture2.Body); if (!flag19) { FP scaleFactor2 = this.MinRays * maxForce * 180f / MathHelper.Pi; CircleShape circleShape2 = fixture2.Shape as CircleShape; bool flag20 = circleShape2 != null; TSVector2 worldPoint; if (flag20) { worldPoint = fixture2.Body.GetWorldPoint(circleShape2.Position); } else { PolygonShape polygonShape2 = fixture2.Shape as PolygonShape; worldPoint = fixture2.Body.GetWorldPoint(polygonShape2.MassData.Centroid); } TSVector2 value4 = scaleFactor2 * (worldPoint - pos); fixture2.Body.ApplyLinearImpulse(ref value4, ref worldPoint); bool flag21 = !dictionary.ContainsKey(fixture2); if (flag21) { dictionary.Add(fixture2, value4); } } } result = dictionary; } return(result); }
public FP GetAngle() { return(FP.Atan2(this.s, this.c)); }