public long getProjectionRadius(Vector2d axis)
{
long pAxisX = Math.Abs(axis.Dot(m_AxisX));
long pAxisY = Math.Abs(axis.Dot(m_AxisY));
return(m_Half.x.Mul(pAxisX) + m_Half.y.Mul(pAxisY));
}
private void ApplyCone(Vector3d center3d, Vector2d forward, long radius, long angle)
{
Vector2d center = center3d.ToVector2d();
long fastRange = radius * radius;
Scan(center, radius);
for (int i = 0; i < ScanOutput.Count; i++)
{
LSAgent agent = ScanOutput[i];
Vector2d agentPos = agent.Body._position;
Vector2d difference = agentPos - center;
if (difference.FastMagnitude() > fastRange)
{
continue;
}
if (forward.Dot(difference) < 0)
{
continue;
}
difference.Normalize();
long cross = forward.Cross(difference).Abs();
if (cross > angle)
{
continue;
}
HitAgent(agent);
}
}
private void HandleCollision(LSBody other)
{
if (!CanMove)
{
return;
}
if ((tempAgent = other.Agent) == null)
{
return;
}
Move otherMover = tempAgent.GetAbility <Move>();
if (ReferenceEquals(otherMover, null) == false)
{
if (IsMoving && CanCollisionStop)
{
if (otherMover.MyMovementGroupID == MyMovementGroupID)
{
if (otherMover.IsMoving == false && otherMover.Arrived && otherMover.stopTime > MinimumOtherStopTime)
{
Arrive();
}
else if (hasPath && otherMover.hasPath && otherMover.pathIndex > 0 && otherMover.lastTargetPos.SqrDistance(targetPos.x, targetPos.y) < FixedMath.One)
{
if (movementDirection.Dot(targetDirection.x, targetDirection.y) < 0)
{
pathIndex++;
}
}
}
}
}
}
public static void PrepareAxisCheck(Vector2d p1, Vector2d p2, bool calculateIntersectionPoints = true)
{
cacheP1 = p1;
cacheP2 = p2;
cacheAxis = p2 - p1;
cacheAxis.Normalize();
cacheAxisNormal = cacheAxis.rotatedLeft;
axisMin = p1.Dot(cacheAxis.x, cacheAxis.y);
axisMax = p2.Dot(cacheAxis.x, cacheAxis.y);
cacheProj = cacheP1.Dot(cacheAxis.x, cacheAxis.y);
cacheProjPerp = cacheP1.Dot(cacheAxisNormal.x, cacheAxisNormal.y);
perpVector = cacheAxisNormal * cacheProjPerp;
calculateIntersections = calculateIntersectionPoints;
}
private void ApplyCone(Vector3d center3d, Vector2d forward, long radius, long angle, Action <LSAgent> apply, PlatformType targetPlatform)
{
Vector2d center = center3d.ToVector2d();
long fastRange = radius * radius;
foreach (LSAgent agent in Scan(center, radius))
{
LSProjectile.agentPos = agent.Body._position;
LSProjectile.difference = LSProjectile.agentPos - center;
if (LSProjectile.difference.FastMagnitude() > fastRange)
{
continue;
}
if (forward.Dot(difference) <= 0)
{
continue;
}
LSProjectile.difference.Normalize();
if (forward.Cross(difference).Abs() > angle)
{
continue;
}
apply(agent);
}
}
public bool isCollision(FixedOBB2D obb)
{
Vector2d centerdis = m_Center - obb.m_Center;
//4条检测轴
Vector2d[] axes =
{
m_AxisX,
m_AxisY,
obb.m_AxisX,
obb.m_AxisY,
};
long r1, r2, r3;
for (int i = 0; i < axes.Length; ++i)
{
r1 = this.getProjectionRadius(axes[i]);
r2 = obb.getProjectionRadius(axes[i]);
r3 = Math.Abs(centerdis.Dot(axes[i]));
if (r1 + r2 <= r3)
{
//Debug.LogFormat("R1_{0:F}, R2_{1:F}, R:{2:F}, R:{3:F}", r1.ToFloat(), r2.ToFloat(), (r1 + r2).ToFloat(), r3.ToFloat());
return(false);
}
}
return(true);
}
public static void PrepareAxisCheck(Vector2d p1, Vector2d p2)
{
cacheP1 = p1;
cacheP2 = p2;
cacheAxis = p2 - p1;
cacheAxis.Normalize();
axisMin = p1.Dot(cacheAxis.x, cacheAxis.y);
axisMax = p2.Dot(cacheAxis.x, cacheAxis.y);
cacheProjPerp = cacheP1.Cross(cacheAxis.x, cacheAxis.y);
}
public bool IsPositionCovered(Vector2d position)
{
//Checks if this body covers a position
//Different techniques for different shapes
switch (this.Shape)
{
case ColliderType.Circle:
long maxDistance = this.Radius + FixedMath.Half;
maxDistance *= maxDistance;
if ((this._position - position).FastMagnitude() > maxDistance)
{
return(false);
}
goto case ColliderType.AABox;
case ColliderType.AABox:
return(position.x + FixedMath.Half >= this.XMin && position.x - FixedMath.Half <= this.XMax &&
position.y + FixedMath.Half >= this.YMin && position.y - FixedMath.Half <= this.YMax);
break;
case ColliderType.Polygon:
for (int i = this.EdgeNorms.Length - 1; i >= 0; i--)
{
Vector2d norm = this.EdgeNorms [i];
long posProj = norm.Dot(position);
long polyMin, polyMax;
CollisionPair.ProjectPolygon(norm.x, norm.y, this, out polyMin, out polyMax);
if (posProj >= polyMin && posProj <= polyMax)
{
}
else
{
return(false);
}
}
return(true);
break;
}
return(false);
}
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 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();
}
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);
}
public static long Dot(Vector2d a, Vector2d b)
{
return(a.Dot(b));
}
