public void GenerateBounds() { if (Shape == ColliderType.Circle) { _radius = Radius; } else if (Shape == ColliderType.AABox) { _radius = FixedMath.Sqrt((HalfHeight * HalfHeight + HalfWidth * HalfWidth) >> FixedMath.SHIFT_AMOUNT); } else if (Shape == ColliderType.Polygon) { long BiggestSqrRadius = Vertices[0].SqrMagnitude(); for (int i = 1; i < Vertices.Length; i++) { long sqrRadius = Vertices[i].SqrMagnitude(); if (sqrRadius > BiggestSqrRadius) { BiggestSqrRadius = sqrRadius; } } _radius = FixedMath.Sqrt(BiggestSqrRadius); FastRadius = this.Radius * this.Radius; } }
/// <summary> /// This vector's magnitude. /// </summary> public long Magnitude() { temp1 = (this.x * this.x + this.y * this.y); if (temp1 == 0) return 0; temp1 >>= FixedMath.SHIFT_AMOUNT; return FixedMath.Sqrt(temp1); }
public long Distance(long otherX, long otherY) { temp1 = this.x - otherX; temp1 *= temp1; temp2 = this.y - otherY; temp2 *= temp2; return(FixedMath.Sqrt((temp1 + temp2) >> FixedMath.SHIFT_AMOUNT)); }
public void Normalize() { long magnitude = FixedMath.Sqrt(x.Mul(x) + y.Mul(y) + z.Mul(z)); x = x.Div(magnitude); y = y.Div(magnitude); z = z.Div(magnitude); }
/// <summary> /// This vector's magnitude. /// </summary> public long Magnitude() { temp1 = (this.x * this.x + this.y * this.y) >> FixedMath.SHIFT_AMOUNT; if (temp1 == 0) { return(0); } return(FixedMath.Sqrt((this.x * this.x + this.y * this.y) >> FixedMath.SHIFT_AMOUNT)); }
public void CalculateAndExecuteBehaviors() { Move mover; if (movers.Count >= MinGroupSize) { averageCollisionSize = 0; groupPosition = Vector2d.zero; for (int i = 0; i < movers.Count; i++) { mover = movers [i]; groupPosition += mover.Position; averageCollisionSize += mover.CollisionSize; } groupPosition /= movers.Count; averageCollisionSize /= movers.Count; long biggestSqrDistance = 0; for (int i = 0; i < movers.Count; i++) { long currentSqrDistance = movers [i].Position.SqrDistance(groupPosition.x, groupPosition.y); if (currentSqrDistance > biggestSqrDistance) { long currentDistance = FixedMath.Sqrt(currentSqrDistance); /* * DistDif = currentDistance - Radius; * if (DistDif > MaximumDistDif * MoversCount / 128) { * ExecuteGroupIndividualMove (); * return; * }*/ biggestSqrDistance = currentSqrDistance; radius = currentDistance; } } if (radius == 0) { ExecuteGroupIndividualMove(); return; } long expectedSize = averageCollisionSize.Mul(averageCollisionSize).Mul(FixedMath.One * 2).Mul(movers.Count); long groupSize = radius.Mul(radius); if (groupSize > expectedSize || groupPosition.FastDistance(Destination.x, Destination.y) < (radius * radius)) { ExecuteGroupIndividualMove(); return; } ExecuteGroupMove(); } else { ExecuteIndividualMove(); } }
public Vector3d Normalize() { long magnitude = FixedMath.Sqrt(x.Mul(x) + y.Mul(y) + z.Mul(z)); if (magnitude == 0) { return(Vector3d.zero); } return(new Vector3d(x.Div(magnitude), y.Div(magnitude), z.Div(magnitude))); }
public static Vector3d Normalize(Vector3d v) { long magnitude = FixedMath.Sqrt(v.x.Mul(v.x) + v.y.Mul(v.y) + v.z.Mul(v.z)); if (magnitude == 0) { return(Vector3d.zero); } return(new Vector3d(v.x.Div(magnitude), v.y.Div(magnitude), v.z.Div(magnitude))); }
Vector2d GetAdjustVector(Vector2d desiredVel) { var adjust = desiredVel - cachedBody._velocity; var adjustFastMag = adjust.FastMagnitude(); //Cap acceleration vector magnitude if (adjustFastMag > timescaledAcceleration * (timescaledAcceleration)) { var mag = FixedMath.Sqrt(adjustFastMag >> FixedMath.SHIFT_AMOUNT); adjust *= timescaledAcceleration.Div(mag); } return(adjust); }
public static long Distance(Vector3d a, Vector3d b) { long tX = b.x - a.x; tX *= tX; tX >>= FixedMath.SHIFT_AMOUNT; long tY = b.y - a.y; tY *= tY; tY >>= FixedMath.SHIFT_AMOUNT; long tZ = b.z - a.z; tZ *= tZ; tZ >>= FixedMath.SHIFT_AMOUNT; return(FixedMath.Sqrt(tX + tY + tZ)); }
public long Distance(Vector3d other) { long tX = other.x - x; tX *= tX; tX >>= FixedMath.SHIFT_AMOUNT; long tY = other.y - y; tY *= tY; tY >>= FixedMath.SHIFT_AMOUNT; long tZ = other.z - z; tZ *= tZ; tZ >>= FixedMath.SHIFT_AMOUNT; return(FixedMath.Sqrt(tX + tY + tZ)); }
public static Vector2d GenerateRandomPointOnCircle(bool evenDistribution = false) { long angle = LSUtility.GetRandomOne().Mul(FixedMath.TwoPi); long distance = LSUtility.GetRandomOne(); if (evenDistribution) { distance = FixedMath.Sqrt(distance); } Vector2d randomOffset = new Vector2d( FixedMath.Trig.Cos(angle), FixedMath.Trig.Sin(angle) ) * distance; return(randomOffset); }
public void ClampMagnitude(long min, long max) { long mag = this.FastMagnitude(); long fastMin; long fastMax; long normal; //Check if normalization is needed and if so, set scale to normalize to if (mag < (fastMin = min * min)) { normal = fastMin; } else if (mag > (fastMax = max * max)) { normal = fastMax; } else { return; } mag = FixedMath.Sqrt(mag >> FixedMath.SHIFT_AMOUNT); if (mag.MoreThanEpsilon()) { //convert from fast multiplied value normal = FixedMath.Sqrt(normal >> FixedMath.SHIFT_AMOUNT); //Shift unneeded as fixed fraction canceled through mul then div x = x * normal / mag; y = y * normal / mag; } else { x = 0; y = 0; } }
public void DistributeCircle_Box(LSBody box, LSBody circle) { xMore = circle.Position.x > box.Position.x; yMore = circle.Position.y > box.Position.y; if (xMore) { PenetrationX = (circle.XMin - box.XMax); if (PenetrationX > 0) { PenetrationX = 0; } } else { PenetrationX = (circle.XMax - box.XMin); if (PenetrationX < 0) { PenetrationX = 0; } } if (yMore) { PenetrationY = (circle.YMin - box.YMax); if (PenetrationY > 0) { PenetrationY = 0; } } else { PenetrationY = (circle.YMax - box.YMin); if (PenetrationY < 0) { PenetrationY = 0; } } xAbs = PenetrationX < 0 ? -PenetrationX : PenetrationX; yAbs = PenetrationY < 0 ? -PenetrationY : PenetrationY; if (xAbs > yAbs) { PenetrationX = 0; } else { PenetrationY = 0; } //Resolving if (Vector2d.Dot(PenetrationX, PenetrationY, circle.Velocity.x, circle.Velocity.y) < 0) { return; } circle.Position.x -= PenetrationX; //(PenetrationX * Multiplier) >> FixedMath.SHIFT_AMOUNT; circle.Position.y -= PenetrationY; //(PenetrationY * Multiplier) >> FixedMath.SHIFT_AMOUNT; if (LeCollisionType != CollisionType.Circle_AABox) { Mag = FixedMath.Sqrt((PenetrationX * PenetrationX + PenetrationY * PenetrationY) >> FixedMath.SHIFT_AMOUNT); PenetrationX = (PenetrationX << FixedMath.SHIFT_AMOUNT) / Mag; PenetrationY = (PenetrationY << FixedMath.SHIFT_AMOUNT) / Mag; } else { if (PenetrationX != 0) { PenetrationX = PenetrationX > 0 ? FixedMath.One : -FixedMath.One; } else if (PenetrationY != 0) { PenetrationY = PenetrationY > 0 ? FixedMath.One : -FixedMath.One; } } circle.PositionChanged = true; circle.BuildBounds(); }
internal void DistributeCollision() { if (!DoPhysics) { return; } switch (LeCollisionType) { case CollisionType.Circle_Circle: dist = FixedMath.Sqrt(FastDistance >> FixedMath.SHIFT_AMOUNT); depth = (Body1.Radius + Body2.Radius - dist); if (depth <= 0) { return; } if (dist == 0) { dist = 1; } //Minimum vector to no longer be colliding DistX = (DistX * depth / dist); DistY = (DistY * depth / dist); //Resolving collision //Note: Immovable bodies don't check collision against each other so this case doesn't need to be considered if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority)) { //Invert adjustment values when Body2 is being moved DistX *= -1; DistY *= -1; DistributeCircle_CirclePriority(Body1, Body2); } else { if (OnlyAffectBody1) { return; } if (Body2.Immovable || Body2.Priority > Body1.Priority) { DistributeCircle_CirclePriority(Body2, Body1); } else { DistX /= 2; DistY /= 2; DistributeCircle(Body1); DistX *= -1; DistY *= -1; DistributeCircle(Body2); } } break; case CollisionType.Circle_AABox: if (Body1.Shape == ColliderType.AABox) { DistributeCircle_Box(Body1, Body2); } else { DistributeCircle_Box(Body2, Body1); } break; case CollisionType.Circle_Polygon: if (Body1.Shape == ColliderType.Circle) { this.DistributeCircle_Poly(Body1, Body2); } else { this.DistributeCircle_Poly(Body2, Body1); } break; } }
public long Magnitude() { long magnitude = FixedMath.Sqrt(x.Mul(x) + y.Mul(y) + z.Mul(z)); return(magnitude); }
private void DistributeCollision() { if (!DoPhysics) { return; } switch (LeCollisionType) { case CollisionType.Circle_Circle: DistX = Body1._position.x - Body2._position.x; DistY = Body1._position.y - Body2._position.y; dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT); if (dist == 0) { //If objects are on the same position, give them push in random direction const long randomMax = FixedMath.One / 32; Body1._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2; Body1._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2; Body1.PositionChanged = true; Body2._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2; Body2._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2; Body2.PositionChanged = true; return; } depth = (Body1.Radius + Body2.Radius - dist); if (depth <= 0) { return; } DistX = (DistX * depth / dist); DistY = (DistY * depth / dist); //Resolving collision if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority)) { DistX *= -1; DistY *= -1; DistributeCircle_CirclePriority(Body1, Body2); } else if (Body2.Immovable || Body2.Priority > Body1.Priority) { DistributeCircle_CirclePriority(Body2, Body1); } else { DistX /= 2; DistY /= 2; DistributeCircle(Body1); DistX *= -1; DistY *= -1; DistributeCircle(Body2); } break; case CollisionType.Circle_AABox: if (Body1.Shape == ColliderType.AABox) { DistributeCircle_Box(Body1, Body2); } else { DistributeCircle_Box(Body2, Body1); } break; case CollisionType.Circle_Polygon: if (Body1.Shape == ColliderType.Circle) { this.DistributeCircle_Poly(Body1, Body2); } else { this.DistributeCircle_Poly(Body2, Body1); } break; } }
private void DistributeCollision() { if (!DoPhysics) { return; } switch (LeCollisionType) { case CollisionType.Circle_Circle: DistX = Body1._position.x - Body2._position.x; DistY = Body1._position.y - Body2._position.y; dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT); if (dist == 0) { //If objects are on the same position, give them push in random direction const long randomMax = FixedMath.One / 32; Body1._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2; Body1._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2; Body1.PositionChanged = true; Body2._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2; Body2._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2; Body2.PositionChanged = true; return; } depth = (Body1.Radius + Body2.Radius - dist); if (depth <= 0) { return; } DistX = (DistX * depth / dist) / 2L; DistY = (DistY * depth / dist) / 2L; //Switch, used to be const bool applyVelocity = false; //Resolving collision //TODO: Less copy-paste code if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority)) { DistX *= -1; DistY *= -1; if (Body1.Immovable || Body2.ImmovableCollisionDirection.EqualsZero()) { Body2._position.x += DistX; Body2._position.y += DistY; Body2.PositionChanged = true; Body2.ImmovableCollisionDirection = new Vector2d(DistX, DistY); if (applyVelocity) { Body2._velocity.x += DistX; Body2._velocity.y += DistY; Body2.VelocityChanged = true; } } else { //Only move if there isn't an immovable object in that direction if (Body2.ImmovableCollisionDirection.x.Sign() != DistX.Sign()) { Body1._position.x += DistX; } if (Body2.ImmovableCollisionDirection.y.Sign() != DistY.Sign()) { Body1._position.y += DistY; } } } else if (Body2.Immovable || Body2.Priority > Body1.Priority) { if (Body2.Immovable || Body1.ImmovableCollisionDirection.EqualsZero()) { Body2.ImmovableCollisionDirection = new Vector2d(DistX, DistY); Body1._position.x += DistX; Body1._position.y += DistY; Body1.PositionChanged = true; if (applyVelocity) { Body1._velocity.x += DistX; Body1._velocity.y += DistY; Body1.VelocityChanged = true; } } else { //Only move if there isn't an immovable object in that direction if (Body1.ImmovableCollisionDirection.x.Sign() != DistX.Sign()) { Body2._position.x += DistX; } if (Body1.ImmovableCollisionDirection.y.Sign() != DistY.Sign()) { Body2._position.y += DistY; } } } else { DistX /= 2; DistY /= 2; Body1._position.x += DistX; Body1._position.y += DistY; Body2._position.x -= DistX; Body2._position.y -= DistY; Body1.PositionChanged = true; Body2.PositionChanged = true; if (applyVelocity) { DistX /= 8; DistY /= 8; Body1._velocity.x += DistX; Body1._velocity.y += DistY; Body1.VelocityChanged = true; Body2._velocity.x -= DistX; Body2._velocity.y -= DistY; Body2.VelocityChanged = true; } } break; case CollisionType.Circle_AABox: if (Body1.Shape == ColliderType.AABox) { DistributeCircle_Box(Body1, Body2); } else { DistributeCircle_Box(Body2, Body1); } break; case CollisionType.Circle_Polygon: if (Body1.Shape == ColliderType.Circle) { this.DistributeCircle_Poly(Body1, Body2); } else { this.DistributeCircle_Poly(Body2, Body1); } break; } }
private void DistributeCollision() { if (Body1.OnContact != null) { Body1.OnContact(Body2); } if (Body2.OnContact != null) { Body2.OnContact(Body1); } if (DoPhysics && Body1.HasParent == false && Body2.HasParent == false) { switch (LeCollisionType) { case CollisionType.Circle_Circle: DistX = Body1.Position.x - Body2.Position.x; DistY = Body1.Position.y - Body2.Position.y; dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT); if (dist == 0) { Body1.Position.x += (LSUtility.GetRandom(1 << (FixedMath.SHIFT_AMOUNT - 2)) + 1); Body1.Position.y -= (LSUtility.GetRandom(1 << (FixedMath.SHIFT_AMOUNT - 2)) + 1); return; } depth = (Body1.Radius + Body2.Radius - dist); if (depth < 0) { return; } DistX = (DistX * depth / dist); DistY = (DistY * depth / dist); //Resolving collision if (physicsFavor == PhysicsFavor.Favor1) { Body2.Position.x -= DistX; Body2.Position.y -= DistY; Body2.PositionChanged = true; } else if (physicsFavor == PhysicsFavor.Favor2) { Body1.Position.x += DistX; Body1.Position.y += DistY; Body1.PositionChanged = true; } else { DistX /= 4; DistY /= 4; if (Body1.Velocity.Dot(Body2.Velocity.x, Body2.Velocity.y) <= 0) { Body1.Velocity.x += DistX; //FixedMath.Mul(DistX, Body1.VelocityMagnitude); Body1.Velocity.y += DistY; //FixedMath.Mul(DistY, Body1.VelocityMagnitude); Body1.VelocityChanged = true; Body2.Velocity.x -= DistX; //FixedMath.Mul(DistX, Body2.VelocityMagnitude); Body2.Velocity.y -= DistY; //FixedMath.Mul(DistY, Body2.VelocityMagnitude); Body2.VelocityChanged = true; Body1.Position.x += DistX; Body1.Position.y += DistY; Body2.Position.x -= DistX; Body2.Position.y -= DistY; } else { Body1.Position.x += DistX; Body1.Position.y += DistY; Body2.Position.x -= DistX; Body2.Position.y -= DistY; } Body1.PositionChanged = true; Body2.PositionChanged = true; } break; case CollisionType.Circle_AABox: if (Body1.Shape == ColliderType.AABox) { DistributeCircle_Box(Body1, Body2); } else { DistributeCircle_Box(Body2, Body1); } break; case CollisionType.Circle_Polygon: break; } } }
private bool InternalRaycast(Vector2d From, Vector2d To, int ExceptionID) { _Version++; MadeContact = false; Hits.FastClear(); const int StepSize = 1 << Partition.ShiftSize; x0 = From.x; y0 = From.y; x1 = To.x; y1 = To.y; if (y1 > y0) { compare1 = y1 - y0; } else { compare1 = y0 - y1; } if (x1 > x0) { compare2 = x1 - x0; } else { compare2 = x0 - x1; } steep = compare1 > compare2; if (steep) { t = x0; // swap x0 and y0 x0 = y0; y0 = t; t = x1; // swap x1 and y1 x1 = y1; y1 = t; } if (x0 > x1) { t = x0; // swap x0 and x1 x0 = x1; x1 = t; t = y0; // swap y0 and y1 y0 = y1; y1 = t; } dx = x1 - x0; dy = (y1 - y0); if (dy < 0) { dy = -dy; } error = dx / 2; ystep = (y0 < y1) ? StepSize : -StepSize; y = y0; AxisX = From.x - To.x; AxisY = From.y - To.y; Mag = FixedMath.Sqrt((AxisX * AxisX + AxisY * AxisY) >> FixedMath.SHIFT_AMOUNT); if (Mag == 0) { return(false); } AxisX = FixedMath.Div(AxisX, Mag); AxisY = FixedMath.Div(AxisY, Mag); AxisMin = Vector2d.Dot(AxisX, AxisY, From.x, From.y); AxisMax = Vector2d.Dot(AxisX, AxisY, To.x, To.y); if (AxisMin > AxisMax) { SwapValue = AxisMin; AxisMin = AxisMax; AxisMax = SwapValue; } PerpProj = Vector2d.Dot(-AxisY, AxisX, From.x, From.y); XMin = From.x; XMax = To.x; if (XMin > XMax) { SwapValue = XMin; XMin = XMax; XMax = SwapValue; } YMin = From.y; YMax = To.y; if (YMin > YMax) { SwapValue = YMin; YMin = YMax; YMax = SwapValue; } x = x0; while (true) { if (steep) { retX = (y - Partition.OffsetX) / StepSize; retY = (x - Partition.OffsetY) / StepSize; } else { retX = (x - Partition.OffsetX) / StepSize; retY = (y - Partition.OffsetY) / StepSize; } PartitionNode node = Partition.Nodes [retX * Partition.Count + retY]; if (node.Count > 0) { for (i = 0; i < node.Count; i++) { DidHit = false; LSBody body = PhysicsManager.SimObjects [node [i]]; if (body.RaycastVersion != _Version && body.ID != ExceptionID) { body.RaycastVersion = _Version; switch (body.Shape) { case ColliderType.Circle: Projection = Vector2d.Dot(AxisX, AxisY, body.Position.x, body.Position.y); TestMin = Projection - body.Radius; TestMax = Projection + body.Radius; if (TestMin < AxisMax) { if (TestMax > AxisMin) { Projection = Vector2d.Dot(-AxisY, AxisX, body.Position.x, body.Position.y); TestMin = Projection - body.Radius; TestMax = Projection + body.Radius; if (PerpProj < TestMax && PerpProj > TestMin) { DidHit = true; } } } break; case ColliderType.AABox: if (AxisMin < body.XMax) { if (AxisMax > body.XMin) { if (PerpProj < body.YMax) { if (PerpProj > body.YMin) { DidHit = true; } } } } break; } if (DidHit) { Hits.Add(body); MadeContact = true; break; } } } } error = error - dy; if (error < 0) { y += ystep; error += dx; } if (x >= x1) { break; } x += StepSize; } return(MadeContact); }
public bool Overlaps(FastList <Vector2d> outputIntersectionPoints) { outputIntersectionPoints.FastClear(); //Checks if this object overlaps the line formed by p1 and p2 switch (this.Shape) { case ColliderType.Circle: { bool overlaps = false; //Check if the circle completely fits between the line long projPos = this._position.Dot(cacheAxis.x, cacheAxis.y); //Circle withing bounds? if (projPos >= axisMin && projPos <= axisMax) { long projPerp = this._position.Dot(cacheAxisNormal.x, cacheAxisNormal.y); long perpDif = (cacheProjPerp - projPerp); long perpDist = perpDif.Abs(); if (perpDist <= _radius) { overlaps = true; } if (overlaps) { long sin = (perpDif); long cos = FixedMath.Sqrt(_radius.Mul(_radius) - sin.Mul(sin)); if (cos == 0) { outputIntersectionPoints.Add((cacheAxis * projPos) + perpVector); } else { outputIntersectionPoints.Add(cacheAxis * (projPos - cos) + perpVector); outputIntersectionPoints.Add(cacheAxis * (projPos + cos) + perpVector); } } } else { //If not, check distances to points long p1Dist = _position.FastDistance(cacheP1.x, cacheP2.y); if (p1Dist <= this.FastRadius) { outputIntersectionPoints.Add(cacheP1); overlaps = true; } long p2Dist = _position.FastDistance(cacheP2.x, cacheP2.y); if (p2Dist <= this.FastRadius) { outputIntersectionPoints.Add(cacheP2); overlaps = true; } } return(overlaps); } break; case ColliderType.AABox: { } break; case ColliderType.Polygon: { bool intersected = false; for (int i = 0; i < this.Vertices.Length; i++) { int edgeIndex = i; Vector2d pivot = this.RealPoints [edgeIndex]; Vector2d edge = this.Edges [edgeIndex]; long proj1 = 0; int nextIndex = edgeIndex + 1 < this.RealPoints.Length ? edgeIndex + 1 : 0; Vector2d nextPoint = RealPoints [nextIndex]; long proj2 = (nextPoint - pivot).Dot(edge); long min; long max; if (proj1 < proj2) { min = proj1; max = proj2; } else { min = proj2; max = proj1; } long lineProj1 = (cacheP1 - pivot).Dot(edge); long lineProj2 = (cacheP2 - pivot).Dot(edge); long lineMin; long lineMax; if (lineProj1 < lineProj2) { lineMin = lineProj1; lineMax = lineProj2; } else { lineMin = lineProj2; lineMax = lineProj1; } if (CollisionPair.CheckOverlap(min, max, lineMin, lineMax)) { Vector2d edgeNorm = this.EdgeNorms [edgeIndex]; long normProj = 0; long normLineProj1 = (cacheP1 - pivot).Dot(edgeNorm); long normLineProj2 = (cacheP2 - pivot).Dot(edgeNorm); long normLineMin; long normLineMax; if (normLineProj1 < normLineProj2) { normLineMin = normLineProj1; normLineMax = normLineProj2; } else { normLineMin = normLineProj2; normLineMax = normLineProj1; } if (normProj >= normLineMin && normProj <= normLineMax) { long revProj1 = pivot.Dot(LSBody.cacheAxisNormal); long revProj2 = nextPoint.Dot(cacheAxisNormal); long revMin; long revMax; if (revProj1 < revProj2) { revMin = revProj1; revMax = revProj2; } else { revMin = revProj2; revMax = revProj1; } if (LSBody.cacheProjPerp >= revMin && LSBody.cacheProjPerp <= revMax) { intersected = true; if (LSBody.calculateIntersections) { long fraction = normLineProj1.Abs().Div(normLineMax - normLineMin); long intersectionProj = FixedMath.Lerp(lineProj1, lineProj2, fraction); outputIntersectionPoints.Add(edge * intersectionProj + pivot); if (outputIntersectionPoints.Count == 2) { break; } } } } } } return(intersected); } break; } return(false); }
private void DistributeCollision() { if (Body1.OnContact.IsNotNull()) { Body1.OnContact(Body2); } if (Body2.OnContact.IsNotNull()) { Body2.OnContact(Body1); } if (Body1.IsTrigger || Body2.IsTrigger) { return; } switch (LeCollisionType) { case CollisionType.Circle_Circle: DistX = Body1._position.x - Body2._position.x; DistY = Body1._position.y - Body2._position.y; dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT); if (dist == 0) { const int randomMax = (int)((long)int.MaxValue % (FixedMath.One / 64)); Body1._position.x += LSUtility.GetRandom(randomMax) - randomMax / 2; Body1._position.y += LSUtility.GetRandom(randomMax) - randomMax / 2; Body1.PositionChanged = true; Body2._position.x += LSUtility.GetRandom(randomMax) - randomMax / 2; Body2._position.y += LSUtility.GetRandom(randomMax) - randomMax / 2; Body2.PositionChanged = true; return; } depth = (Body1.Radius + Body2.Radius - dist); if (depth <= 0) { return; } DistX = (DistX * depth / dist) / 2L; DistY = (DistY * depth / dist) / 2L; const bool applyVelocity = false; //Resolving collision if (Body1.Immovable && Body1.isActiveAndEnabled || (Body2.Immovable == false && Body1.Priority > Body2.Priority)) { Body2._position.x -= DistX; Body2._position.y -= DistY; Body2.PositionChanged = true; if (applyVelocity) { Body2._velocity.x -= DistX; Body2.VelocityChanged = true; } } else if (Body2.Immovable || Body2.Priority > Body1.Priority) { Body1._position.x += DistX; Body1._position.y += DistY; Body1.PositionChanged = true; if (applyVelocity) { Body1._velocity.x += DistX; Body1._velocity.y += DistY; Body1.VelocityChanged = true; } } else { DistX /= 2; DistY /= 2; Body1._position.x += DistX; Body1._position.y += DistY; Body2._position.x -= DistX; Body2._position.y -= DistY; Body1.PositionChanged = true; Body2.PositionChanged = true; if (applyVelocity) { DistX /= 8; DistY /= 8; Body1._velocity.x += DistX; Body1._velocity.y += DistY; Body1.VelocityChanged = true; Body2._velocity.x -= DistX; Body2._velocity.y -= DistY; Body2.VelocityChanged = true; } } break; case CollisionType.Circle_AABox: if (Body1.Shape == ColliderType.AABox) { DistributeCircle_Box(Body1, Body2); } else { DistributeCircle_Box(Body2, Body1); } break; case CollisionType.Circle_Polygon: if (Body1.Shape == ColliderType.Circle) { this.DistributeCircle_Poly(Body1, Body2); } else { this.DistributeCircle_Poly(Body2, Body1); } break; } }
void BehaveWithTarget() { if (Target.IsActive == false || Target.SpawnVersion != targetVersion) { StopEngage(); BehaveWithNoTarget(); return; } Vector2d targetDirection = Target.Body.Position - cachedBody.Position; long fastMag = targetDirection.FastMagnitude(); if (fastMag <= fastRangeToTarget) { if (!inRange) { if (CanMove) { cachedMove.StopMove(); } } Agent.SetState(AnimState.Engaging); long mag = FixedMath.Sqrt(fastMag >> FixedMath.SHIFT_AMOUNT); //cachedTurn.StartTurn(targetDirection / mag); bool withinTurn = TrackAttackAngle == false || (fastMag != 0 && cachedBody.Rotation.Dot(targetDirection.x, targetDirection.y) > 0 && cachedBody.Rotation.Cross(targetDirection.x, targetDirection.y).Abs() <= AttackAngle); bool needTurn = mag != 0 && !withinTurn; if (needTurn) { if (CanTurn) { targetDirection /= mag; cachedTurn.StartTurn(targetDirection); } } else { if (attackCount <= 0) { attackCount = attackFrameCount; Fire(); } } if (inRange == false) { inRange = true; } } else { if (CanMove) { if (cachedMove.IsMoving == false) { cachedMove.StartMove(Target.Body.Position); cachedBody.Priority = basePriority; } else { if (Target.Body.PositionChanged || inRange) { cachedMove.Destination = Target.Body.Position; } } } if (isAttackMoving || isFocused == false) { searchCount -= 1; if (searchCount <= 0) { searchCount = SearchRate; if (ScanAndEngage()) { } else { } } } if (inRange == true) { inRange = false; } } }
public void Initialize(LSBody b1, LSBody b2) { IsValid = true; if (!IsValid) { return; } if (b1.ID < b2.ID) { Body1 = b1; Body2 = b2; } else { Body1 = b2; Body2 = b1; } _ranIndex = -1; _isColliding = false; DistX = 0; DistY = 0; PenetrationX = 0; PenetrationY = 0; FastCollideDistance = b1.Radius + b2.Radius; FastCollideDistance *= FastCollideDistance; LeCollisionType = CollisionType.None; if (Body1.Shape == ColliderType.None || Body2.Shape == ColliderType.None) { } else if (Body1.Shape == ColliderType.Circle) { if (Body2.Shape == ColliderType.Circle) { LeCollisionType = CollisionType.Circle_Circle; } else if (Body2.Shape == ColliderType.AABox) { LeCollisionType = CollisionType.Circle_AABox; } else if (Body2.Shape == ColliderType.Polygon) { LeCollisionType = CollisionType.Circle_Polygon; } } else if (Body1.Shape == ColliderType.AABox) { if (Body2.Shape == ColliderType.Circle) { LeCollisionType = CollisionType.Circle_AABox; } else if (Body2.Shape == ColliderType.AABox) { LeCollisionType = CollisionType.AABox_AABox; } else if (Body2.Shape == ColliderType.Polygon) { LeCollisionType = CollisionType.AABox_Polygon; } } else if (Body1.Shape == ColliderType.Polygon) { if (Body2.Shape == ColliderType.Circle) { LeCollisionType = CollisionType.Circle_Polygon; } else if (Body2.Shape == ColliderType.AABox) { LeCollisionType = CollisionType.AABox_Polygon; } else if (Body2.Shape == ColliderType.Polygon) { LeCollisionType = CollisionType.Polygon_Polygon; } } DoPhysics = ((Body1.IsTrigger || Body2.IsTrigger) == false); if (DoPhysics) { } //TODO: Space out checks when culled //TODO: The time between collision checks might cause goofy behavior //Maybe use a distance or velocity heuristic for culling instead of time since last collision //It wouldn't be able to replace partitions because of raycasts and fast-moving objects //Let's see if this works well or if something better is needed. if (Body1.PreventCulling || Body2.PreventCulling) { //Never cull CullCounter = -1; } else { //Immediately check collision CullCounter = 0; //If collision distance is too large, don't cull based on distance PreventDistanceCull = FastCollideDistance > PhysicsManager.CullFastDistanceMax; LastCollidedFrame = LockstepManager.FrameCount; FastDistanceOffset = FixedMath.Sqrt(FastCollideDistance >> FixedMath.SHIFT_AMOUNT) + FixedMath.One * 2; FastDistanceOffset *= FastDistanceOffset; } Active = true; _Version++; }
/// <summary> /// This vector's magnitude. /// </summary> public long Magnitude() { return(FixedMath.Sqrt((this.x * this.x + this.y * this.y) >> FixedMath.SHIFT_AMOUNT)); }
public static long Cos(long theta) { long sin = Sin(theta); return(FixedMath.Sqrt(FixedMath.One - (sin.Mul(sin)))); }
public void DistributeCollision() { if (!Active) { return; } if (IsColliding) { if (Body1.OnContact != null) { Body1.OnContact(Body2); } if (Body2.OnContact != null) { Body2.OnContact(Body1); } if (DoPhysics) { switch (LeCollisionType) { case CollisionType.Circle_Circle: DistX = Body1.Position.x - Body2.Position.x; DistY = Body1.Position.y - Body2.Position.y; dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT); if (dist == 0) { Body1.Position.x += (LSUtility.GetRandom(1 << (FixedMath.SHIFT_AMOUNT - 2)) + 1); Body1.Position.y -= (LSUtility.GetRandom(1 << (FixedMath.SHIFT_AMOUNT - 2)) + 1); Body1.PositionChanged = true; return; } depth = (Body1.Radius + Body2.Radius - dist); if (depth < 0) { return; } DistX = (DistX * depth / dist); DistY = (DistY * depth / dist); //Resolving collision if (Body1.Immovable) { Body2.Position.x -= DistX; Body2.Position.y -= DistY; Body2.PositionChanged = true; } else if (Body2.Immovable) { Body1.Position.x += DistX; Body1.Position.y += DistY; Body1.PositionChanged = true; } else { DistX /= 4; DistY /= 4; if (Body1.Velocity.Dot(Body2.Velocity.x, Body2.Velocity.y) < 0) { if (Body1.Mover != null) { Body1.Velocity.x += DistX; Body1.Velocity.y += DistY; Body1.VelocityChanged = true; } else { Body1.Position.x += DistX; Body1.Position.y += DistY; Body1.PositionChanged = true; } if (Body2.Mover != null) { Body2.Velocity.x -= DistX; Body2.Velocity.y -= DistY; Body2.VelocityChanged = true; } else { Body2.Position.x -= DistX; Body2.Position.y -= DistY; Body2.PositionChanged = true; } } else { Body1.Position.x += DistX; Body1.Position.y += DistY; Body1.PositionChanged = true; Body2.Position.x -= DistX; Body2.Position.y -= DistY; Body2.PositionChanged = true; } } break; case CollisionType.Circle_AABox: if (Body1.Shape == ColliderType.AABox) { DistributeCircle_Box(Body1, Body2); } else { DistributeCircle_Box(Body2, Body1); } break; case CollisionType.Circle_Polygon: break; } } } }
private void DistributeCollision() { if (Body1.OnContact.IsNotNull()) { Body1.OnContact(Body2); } if (Body2.OnContact.IsNotNull()) { Body2.OnContact(Body1); } if (DoPhysics && Body1.HasParent == false && Body2.HasParent == false) { switch (LeCollisionType) { case CollisionType.Circle_Circle: DistX = Body1.Position.x - Body2.Position.x; DistY = Body1.Position.y - Body2.Position.y; dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT); if (dist == 0) { const int randomMax = 1000; Body1.Position.x += LSUtility.GetRandom(randomMax); Body1.Position.y += LSUtility.GetRandom(randomMax); Body1.PositionChanged = true; Body2.Position.x += LSUtility.GetRandom(randomMax); Body2.Position.y += LSUtility.GetRandom(randomMax); Body2.PositionChanged = true; return; } depth = (Body1.Radius + Body2.Radius - dist); if (depth <= 0) { return; } DistX = (DistX * depth / dist) / 2L; DistY = (DistY * depth / dist) / 2L; const bool applyVelocity = true; //Resolving collision if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority)) { Body2.Position.x -= DistX; Body2.Position.y -= DistY; Body2.PositionChanged = true; if (applyVelocity) { Body2._velocity.x -= DistX; Body2._velocity.y -= DistY; Body2.VelocityChanged = true; } } else if (Body2.Immovable || Body2.Priority > Body1.Priority) { Body1.Position.x += DistX; Body1.Position.y += DistY; Body1.PositionChanged = true; if (applyVelocity) { Body1._velocity.x += DistX; Body1._velocity.y += DistY; Body1.VelocityChanged = true; } } else { DistX /= 2; DistY /= 2; Body1.Position.x += DistX; Body1.Position.y += DistY; Body2.Position.x -= DistX; Body2.Position.y -= DistY; Body1.PositionChanged = true; Body2.PositionChanged = true; if (applyVelocity) { DistX /= 8; DistY /= 8; Body1._velocity.x += DistX; Body1._velocity.y += DistY; Body1.VelocityChanged = true; Body2._velocity.x -= DistX; Body2._velocity.y -= DistY; Body2.VelocityChanged = true; } } break; case CollisionType.Circle_AABox: if (Body1.Shape == ColliderType.AABox) { DistributeCircle_Box(Body1, Body2); } else { DistributeCircle_Box(Body2, Body1); } break; case CollisionType.Circle_Polygon: break; } } }
public static long Distance(Vector2d a, Vector2d b) { return(FixedMath.Sqrt(SqrDistance(a, b))); }