public Vector2Int WorldPosCeilingGridPoint(TSVector2 pos) { pos -= SDF.Origin; pos /= SDF.Grain; ushort x = (ushort)FP.Ceiling(pos.x); ushort y = (ushort)FP.Ceiling(pos.y); return(new Vector2Int(x, y)); }
/// <summary> /// Passes a axis aligned bounding box to the shape where collision /// could occour. /// </summary> /// <param name="box">The bounding box where collision could occur.</param> /// <returns>The upper index with which <see cref="SetCurrentShape"/> can be /// called.</returns> public override int Prepare(ref TSBBox box) { // simple idea: the terrain is a grid. x and z is the position in the grid. // y the height. we know compute the min and max grid-points. All quads // between these points have to be checked. // including overflow exception prevention if (box.min.x < boundings.min.x) { minX = 0; } else { minX = (int)FP.Floor(((box.min.x - sphericalExpansion) / scaleX)); minX = (int)TSMath.Max(minX, 0); } if (box.max.x > boundings.max.x) { maxX = heightsLength0 - 1; } else { maxX = (int)FP.Ceiling(((box.max.x + sphericalExpansion) / scaleX)); maxX = (int)TSMath.Min(maxX, heightsLength0 - 1); } if (box.min.z < boundings.min.z) { minZ = 0; } else { minZ = (int)FP.Floor(((box.min.z - sphericalExpansion) / scaleZ)); minZ = (int)TSMath.Max(minZ, 0); } if (box.max.z > boundings.max.z) { maxZ = heightsLength1 - 1; } else { maxZ = (int)FP.Ceiling((FP)((box.max.z + sphericalExpansion) / scaleZ)); maxZ = (int)TSMath.Min(maxZ, heightsLength1 - 1); } numX = maxX - minX; numZ = maxZ - minZ; // since every quad contains two triangles we multiply by 2. return(numX * numZ * 2); }
/// <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 static Vector2Int CeilingToGrid(this SDFRawData sdf, TSVector2 positin) { positin /= sdf.Grain; return(new Vector2Int((int)FP.Ceiling(positin.x), (int)FP.Ceiling(positin.y))); }