File indexing completed on 2025-08-03 03:49:55

 /*
 * Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
 #include <new>
 
 b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
 {
     void* mem = allocator->Allocate(sizeof(b2PolygonShape));
     b2PolygonShape* clone = new (mem) b2PolygonShape;
     *clone = *this;
     return clone;
 }
 
 void b2PolygonShape::SetAsBox(qreal hx, qreal hy)
 {
     m_vertexCount = 4;
     m_vertices[0].Set(-hx, -hy);
     m_vertices[1].Set( hx, -hy);
     m_vertices[2].Set( hx,  hy);
     m_vertices[3].Set(-hx,  hy);
     m_normals[0].Set(0.0f, -1.0f);
     m_normals[1].Set(1.0f, 0.0f);
     m_normals[2].Set(0.0f, 1.0f);
     m_normals[3].Set(-1.0f, 0.0f);
     m_centroid.SetZero();
 }
 
 void b2PolygonShape::SetAsBox(qreal hx, qreal hy, const b2Vec2& center, qreal angle)
 {
     m_vertexCount = 4;
     m_vertices[0].Set(-hx, -hy);
     m_vertices[1].Set( hx, -hy);
     m_vertices[2].Set( hx,  hy);
     m_vertices[3].Set(-hx,  hy);
     m_normals[0].Set(0.0f, -1.0f);
     m_normals[1].Set(1.0f, 0.0f);
     m_normals[2].Set(0.0f, 1.0f);
     m_normals[3].Set(-1.0f, 0.0f);
     m_centroid = center;
 
     b2Transform xf;
     xf.position = center;
     xf.R.Set(angle);
 
     // Transform vertices and normals.
     for (int32 i = 0; i < m_vertexCount; ++i)
     {
         m_vertices[i] = b2Mul(xf, m_vertices[i]);
         m_normals[i] = b2Mul(xf.R, m_normals[i]);
     }
 }
 
 int32 b2PolygonShape::GetChildCount() const
 {
     return 1;
 }
 
 static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
 {
     b2Assert(count >= 3);
 
     b2Vec2 c; c.Set(0.0f, 0.0f);
     qreal area = 0.0f;
 
     // pRef is the reference point for forming triangles.
     // It's location doesn't change the result (except for rounding error).
     b2Vec2 pRef(0.0f, 0.0f);
 #if 0
     // This code would put the reference point inside the polygon.
     for (int32 i = 0; i < count; ++i)
     {
         pRef += vs[i];
     }
     pRef *= 1.0f / count;
 #endif
 
     const qreal inv3 = 1.0f / 3.0f;
 
     for (int32 i = 0; i < count; ++i)
     {
         // Triangle vertices.
         b2Vec2 p1 = pRef;
         b2Vec2 p2 = vs[i];
         b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
 
         b2Vec2 e1 = p2 - p1;
         b2Vec2 e2 = p3 - p1;
 
         qreal D = b2Cross(e1, e2);
 
         qreal triangleArea = qAbs(0.5f * D);
         area += triangleArea;
 
         // Area weighted centroid
         c += triangleArea * inv3 * (p1 + p2 + p3);
     }
 
     // Centroid
     b2Assert(area > b2_epsilon);
     c *= 1.0f / area;
     return c;
 }
 
 void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
 {
     b2Assert(3 <= count && count <= b2_maxPolygonVertices);
     m_vertexCount = count;
 
     // Copy vertices.
     for (int32 i = 0; i < m_vertexCount; ++i)
     {
         m_vertices[i] = vertices[i];
     }
 
     // Compute normals. Ensure the edges have non-zero length.
     for (int32 i = 0; i < m_vertexCount; ++i)
     {
         int32 i1 = i;
         int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
         b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
         b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
         m_normals[i] = b2Cross(edge, 1.0f);
         m_normals[i].Normalize();
     }
 
 #ifdef _DEBUG
     // Ensure the polygon is convex and the interior
     // is to the left of each edge.
     for (int32 i = 0; i < m_vertexCount; ++i)
     {
         int32 i1 = i;
         int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
         b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
 
         for (int32 j = 0; j < m_vertexCount; ++j)
         {
             // Don't check vertices on the current edge.
             if (j == i1 || j == i2)
             {
                 continue;
             }
             
             b2Vec2 r = m_vertices[j] - m_vertices[i1];
 
             // Your polygon is non-convex (it has an indentation) or
             // has collinear edges.
             qreal s = b2Cross(edge, r);
             b2Assert(s > 0.0f);
         }
     }
 #endif
 
     // Compute the polygon centroid.
     m_centroid = ComputeCentroid(m_vertices, m_vertexCount);
 }
 
 bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
 {
     b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
 
     for (int32 i = 0; i < m_vertexCount; ++i)
     {
         qreal dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
         if (dot > 0.0f)
         {
             return false;
         }
     }
 
     return true;
 }
 
 bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
                                 const b2Transform& xf, int32 childIndex) const
 {
     B2_NOT_USED(childIndex);
 
     // Put the ray into the polygon's frame of reference.
     b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);
     b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
     b2Vec2 d = p2 - p1;
 
     qreal lower = 0.0f, upper = input.maxFraction;
 
     int32 index = -1;
 
     for (int32 i = 0; i < m_vertexCount; ++i)
     {
         // p = p1 + a * d
         // dot(normal, p - v) = 0
         // dot(normal, p1 - v) + a * dot(normal, d) = 0
         qreal numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
         qreal denominator = b2Dot(m_normals[i], d);
 
         if (denominator == 0.0f)
         {   
             if (numerator < 0.0f)
             {
                 return false;
             }
         }
         else
         {
             // Note: we want this predicate without division:
             // lower < numerator / denominator, where denominator < 0
             // Since denominator < 0, we have to flip the inequality:
             // lower < numerator / denominator <==> denominator * lower > numerator.
             if (denominator < 0.0f && numerator < lower * denominator)
             {
                 // Increase lower.
                 // The segment enters this half-space.
                 lower = numerator / denominator;
                 index = i;
             }
             else if (denominator > 0.0f && numerator < upper * denominator)
             {
                 // Decrease upper.
                 // The segment exits this half-space.
                 upper = numerator / denominator;
             }
         }
 
         // The use of epsilon here causes the assert on lower to trip
         // in some cases. Apparently the use of epsilon was to make edge
         // shapes work, but now those are handled separately.
         //if (upper < lower - b2_epsilon)
         if (upper < lower)
         {
             return false;
         }
     }
 
     b2Assert(0.0f <= lower && lower <= input.maxFraction);
 
     if (index >= 0)
     {
         output->fraction = lower;
         output->normal = b2Mul(xf.R, m_normals[index]);
         return true;
     }
 
     return false;
 }
 
 void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
 {
     B2_NOT_USED(childIndex);
 
     b2Vec2 lower = b2Mul(xf, m_vertices[0]);
     b2Vec2 upper = lower;
 
     for (int32 i = 1; i < m_vertexCount; ++i)
     {
         b2Vec2 v = b2Mul(xf, m_vertices[i]);
         lower = b2Min(lower, v);
         upper = b2Max(upper, v);
     }
 
     b2Vec2 r(m_radius, m_radius);
     aabb->lowerBound = lower - r;
     aabb->upperBound = upper + r;
 }
 
 void b2PolygonShape::ComputeMass(b2MassData* massData, qreal density) const
 {
     // Polygon mass, centroid, and inertia.
     // Let rho be the polygon density in mass per unit area.
     // Then:
     // mass = rho * int(dA)
     // centroid.x = (1/mass) * rho * int(x * dA)
     // centroid.y = (1/mass) * rho * int(y * dA)
     // I = rho * int((x*x + y*y) * dA)
     //
     // We can compute these integrals by summing all the integrals
     // for each triangle of the polygon. To evaluate the integral
     // for a single triangle, we make a change of variables to
     // the (u,v) coordinates of the triangle:
     // x = x0 + e1x * u + e2x * v
     // y = y0 + e1y * u + e2y * v
     // where 0 <= u && 0 <= v && u + v <= 1.
     //
     // We integrate u from [0,1-v] and then v from [0,1].
     // We also need to use the Jacobian of the transformation:
     // D = cross(e1, e2)
     //
     // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
     //
     // The rest of the derivation is handled by computer algebra.
 
     b2Assert(m_vertexCount >= 3);
 
     b2Vec2 center; center.Set(0.0f, 0.0f);
     qreal area = 0.0f;
     qreal I = 0.0f;
 
     // pRef is the reference point for forming triangles.
     // It's location doesn't change the result (except for rounding error).
     b2Vec2 pRef(0.0f, 0.0f);
 #if 0
     // This code would put the reference point inside the polygon.
     for (int32 i = 0; i < m_vertexCount; ++i)
     {
         pRef += m_vertices[i];
     }
     pRef *= 1.0f / count;
 #endif
 
     const qreal k_inv3 = 1.0f / 3.0f;
 
     for (int32 i = 0; i < m_vertexCount; ++i)
     {
         // Triangle vertices.
         b2Vec2 p1 = pRef;
         b2Vec2 p2 = m_vertices[i];
         b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
 
         b2Vec2 e1 = p2 - p1;
         b2Vec2 e2 = p3 - p1;
 
         qreal D = b2Cross(e1, e2);
 
         qreal triangleArea = 0.5f * D;
         area += triangleArea;
 
         // Area weighted centroid
         center += triangleArea * k_inv3 * (p1 + p2 + p3);
 
         qreal px = p1.x, py = p1.y;
         qreal ex1 = e1.x, ey1 = e1.y;
         qreal ex2 = e2.x, ey2 = e2.y;
 
         qreal intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
         qreal inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
 
         I += D * (intx2 + inty2);
     }
 
     // Total mass
     massData->mass = density * area;
 
     // Center of mass
     b2Assert(area > b2_epsilon);
     center *= 1.0f / area;
     massData->center = center;
 
     // Inertia tensor relative to the local origin.
     massData->I = density * I;
 }