From 2706365e5014621c405d713c28460ad4a3492f96 Mon Sep 17 00:00:00 2001 From: Nikos Kastellanos Date: Sun, 23 Dec 2018 17:10:36 +0200 Subject: [PATCH] Optimize PolygonShape.ComputeAABB(...) (#62) Optimize PolygonShape.ComputeAABB(...), EdgeShape.ComputeAABB(...) and CircleShape.ComputeAABB(...) --- Physics2D/Collision/Shapes/CircleShape.cs | 13 +++++-- Physics2D/Collision/Shapes/EdgeShape.cs | 44 ++++++++++++++++++---- Physics2D/Collision/Shapes/PolygonShape.cs | 33 ++++++++++++---- 3 files changed, 71 insertions(+), 19 deletions(-) diff --git a/Physics2D/Collision/Shapes/CircleShape.cs b/Physics2D/Collision/Shapes/CircleShape.cs index a87d5116..47bdd84a 100644 --- a/Physics2D/Collision/Shapes/CircleShape.cs +++ b/Physics2D/Collision/Shapes/CircleShape.cs @@ -134,9 +134,16 @@ public override bool RayCast(out RayCastOutput output, ref RayCastInput input, r public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) { - Vector2 p = transform.p + Complex.Multiply(ref _position, ref transform.q); - aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius); - aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius); + // OPT: Vector2 p = transform.p + Complex.Multiply(ref _position, ref transform.q); + var pX = (_position.X * transform.q.Real - _position.Y * transform.q.Imaginary) + transform.p.X; + var pY = (_position.Y * transform.q.Real + _position.X * transform.q.Imaginary) + transform.p.Y; + + // OPT: aabb.LowerBound = new Vector2(p.X - Radius, p.Y - Radius); + // OPT: aabb.UpperBound = new Vector2(p.X + Radius, p.Y + Radius); + aabb.LowerBound.X = pX - Radius; + aabb.LowerBound.Y = pY - Radius; + aabb.UpperBound.X = pX + Radius; + aabb.UpperBound.Y = pY + Radius; } protected override sealed void ComputeProperties() diff --git a/Physics2D/Collision/Shapes/EdgeShape.cs b/Physics2D/Collision/Shapes/EdgeShape.cs index 37174511..07783b48 100644 --- a/Physics2D/Collision/Shapes/EdgeShape.cs +++ b/Physics2D/Collision/Shapes/EdgeShape.cs @@ -207,15 +207,43 @@ public override bool RayCast(out RayCastOutput output, ref RayCastInput input, r public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) { - Vector2 v1 = Transform.Multiply(ref _vertex1, ref transform); - Vector2 v2 = Transform.Multiply(ref _vertex2, ref transform); - - Vector2 lower = Vector2.Min(v1, v2); - Vector2 upper = Vector2.Max(v1, v2); + // OPT: Vector2 v1 = Transform.Multiply(ref _vertex1, ref transform); + float v1X = (_vertex1.X * transform.q.Real - _vertex1.Y * transform.q.Imaginary) + transform.p.X; + float v1Y = (_vertex1.Y * transform.q.Real + _vertex1.X * transform.q.Imaginary) + transform.p.Y; + // OPT: Vector2 v2 = Transform.Multiply(ref _vertex2, ref transform); + float v2X = (_vertex2.X * transform.q.Real - _vertex2.Y * transform.q.Imaginary) + transform.p.X; + float v2Y = (_vertex2.Y * transform.q.Real + _vertex2.X * transform.q.Imaginary) + transform.p.Y; + + // OPT: aabb.LowerBound = Vector2.Min(v1, v2); + // OPT: aabb.UpperBound = Vector2.Max(v1, v2); + if (v1X < v2X) + { + aabb.LowerBound.X = v1X; + aabb.UpperBound.X = v2X; + } + else + { + aabb.LowerBound.X = v2X; + aabb.UpperBound.X = v1X; + } + if (v1Y < v2Y) + { + aabb.LowerBound.Y = v1Y; + aabb.UpperBound.Y = v2Y; + } + else + { + aabb.LowerBound.Y = v2Y; + aabb.UpperBound.Y = v1Y; + } - Vector2 r = new Vector2(Radius, Radius); - aabb.LowerBound = lower - r; - aabb.UpperBound = upper + r; + // OPT: Vector2 r = new Vector2(Radius, Radius); + // OPT: aabb.LowerBound = aabb.LowerBound - r; + // OPT: aabb.UpperBound = aabb.LowerBound + r; + aabb.LowerBound.X -= Radius; + aabb.LowerBound.Y -= Radius; + aabb.UpperBound.X += Radius; + aabb.UpperBound.Y += Radius; } protected override void ComputeProperties() diff --git a/Physics2D/Collision/Shapes/PolygonShape.cs b/Physics2D/Collision/Shapes/PolygonShape.cs index 8cd7b36e..d6b34e26 100644 --- a/Physics2D/Collision/Shapes/PolygonShape.cs +++ b/Physics2D/Collision/Shapes/PolygonShape.cs @@ -317,19 +317,36 @@ public override bool RayCast(out RayCastOutput output, ref RayCastInput input, r /// The child shape index. public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex) { - Vector2 lower = Transform.Multiply(Vertices[0], ref transform); - Vector2 upper = lower; + // OPT: aabb.LowerBound = Transform.Multiply(Vertices[0], ref transform); + var vert = Vertices[0]; + aabb.LowerBound.X = (vert.X * transform.q.Real - vert.Y * transform.q.Imaginary) + transform.p.X; + aabb.LowerBound.Y = (vert.Y * transform.q.Real + vert.X * transform.q.Imaginary) + transform.p.Y; + aabb.UpperBound = aabb.LowerBound; for (int i = 1; i < Vertices.Count; ++i) { - Vector2 v = Transform.Multiply(Vertices[i], ref transform); - Vector2.Min(ref lower, ref v, out lower); - Vector2.Max(ref upper, ref v, out upper); + // OPT: Vector2 v = Transform.Multiply(Vertices[i], ref transform); + vert = Vertices[i]; + float vX = (vert.X * transform.q.Real - vert.Y * transform.q.Imaginary) + transform.p.X; + float vY = (vert.Y * transform.q.Real + vert.X * transform.q.Imaginary) + transform.p.Y; + + // OPT: Vector2.Min(ref aabb.LowerBound, ref v, out aabb.LowerBound); + // OPT: Vector2.Max(ref aabb.UpperBound, ref v, out aabb.UpperBound); + Debug.Assert(aabb.LowerBound.X <= aabb.UpperBound.X); + if (vX < aabb.LowerBound.X) aabb.LowerBound.X = vX; + else if (vX > aabb.UpperBound.X) aabb.UpperBound.X = vX; + Debug.Assert(aabb.LowerBound.Y <= aabb.UpperBound.Y); + if (vY < aabb.LowerBound.Y) aabb.LowerBound.Y = vY; + else if (vY > aabb.UpperBound.Y) aabb.UpperBound.Y = vY; } - Vector2 r = new Vector2(Radius, Radius); - aabb.LowerBound = lower - r; - aabb.UpperBound = upper + r; + // OPT: Vector2 r = new Vector2(Radius, Radius); + // OPT: aabb.LowerBound = aabb.LowerBound - r; + // OPT: aabb.UpperBound = aabb.UpperBound + r; + aabb.LowerBound.X -= Radius; + aabb.LowerBound.Y -= Radius; + aabb.UpperBound.X += Radius; + aabb.UpperBound.Y += Radius; } public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc)