mirror of
https://github.com/pocketpy/pocketpy
synced 2025-10-23 13:00:17 +00:00
367 lines
8.9 KiB
C++
367 lines
8.9 KiB
C++
// MIT License
|
|
|
|
// Copyright (c) 2019 Erin Catto
|
|
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
// of this software and associated documentation files (the "Software"), to deal
|
|
// in the Software without restriction, including without limitation the rights
|
|
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
// copies of the Software, and to permit persons to whom the Software is
|
|
// furnished to do so, subject to the following conditions:
|
|
|
|
// The above copyright notice and this permission notice shall be included in all
|
|
// copies or substantial portions of the Software.
|
|
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
// SOFTWARE.
|
|
|
|
#include "box2d/b2_polygon_shape.h"
|
|
#include "box2d/b2_block_allocator.h"
|
|
|
|
#include <new>
|
|
|
|
b2PolygonShape::b2PolygonShape()
|
|
{
|
|
m_type = e_polygon;
|
|
m_radius = b2_polygonRadius;
|
|
m_count = 0;
|
|
m_centroid.SetZero();
|
|
}
|
|
|
|
b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
|
|
{
|
|
void* mem = allocator->Allocate(sizeof(b2PolygonShape));
|
|
b2PolygonShape* clone = new (mem) b2PolygonShape;
|
|
*clone = *this;
|
|
return clone;
|
|
}
|
|
|
|
void b2PolygonShape::SetAsBox(float hx, float hy)
|
|
{
|
|
m_count = 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(float hx, float hy, const b2Vec2& center, float angle)
|
|
{
|
|
m_count = 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.p = center;
|
|
xf.q.Set(angle);
|
|
|
|
// Transform vertices and normals.
|
|
for (int32 i = 0; i < m_count; ++i)
|
|
{
|
|
m_vertices[i] = b2Mul(xf, m_vertices[i]);
|
|
m_normals[i] = b2Mul(xf.q, m_normals[i]);
|
|
}
|
|
}
|
|
|
|
int32 b2PolygonShape::GetChildCount() const
|
|
{
|
|
return 1;
|
|
}
|
|
|
|
static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
|
|
{
|
|
b2Assert(count >= 3);
|
|
|
|
b2Vec2 c(0.0f, 0.0f);
|
|
float area = 0.0f;
|
|
|
|
// Get a reference point for forming triangles.
|
|
// Use the first vertex to reduce round-off errors.
|
|
b2Vec2 s = vs[0];
|
|
|
|
const float inv3 = 1.0f / 3.0f;
|
|
|
|
for (int32 i = 0; i < count; ++i)
|
|
{
|
|
// Triangle vertices.
|
|
b2Vec2 p1 = vs[0] - s;
|
|
b2Vec2 p2 = vs[i] - s;
|
|
b2Vec2 p3 = i + 1 < count ? vs[i+1] - s : vs[0] - s;
|
|
|
|
b2Vec2 e1 = p2 - p1;
|
|
b2Vec2 e2 = p3 - p1;
|
|
|
|
float D = b2Cross(e1, e2);
|
|
|
|
float triangleArea = 0.5f * D;
|
|
area += triangleArea;
|
|
|
|
// Area weighted centroid
|
|
c += triangleArea * inv3 * (p1 + p2 + p3);
|
|
}
|
|
|
|
// Centroid
|
|
b2Assert(area > b2_epsilon);
|
|
c = (1.0f / area) * c + s;
|
|
return c;
|
|
}
|
|
|
|
bool b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
|
|
{
|
|
b2Hull hull = b2ComputeHull(vertices, count);
|
|
|
|
if (hull.count < 3)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
Set(hull);
|
|
|
|
return true;
|
|
}
|
|
|
|
void b2PolygonShape::Set(const b2Hull& hull)
|
|
{
|
|
b2Assert(hull.count >= 3);
|
|
|
|
m_count = hull.count;
|
|
|
|
// Copy vertices
|
|
for (int32 i = 0; i < hull.count; ++i)
|
|
{
|
|
m_vertices[i] = hull.points[i];
|
|
}
|
|
|
|
// Compute normals. Ensure the edges have non-zero length.
|
|
for (int32 i = 0; i < m_count; ++i)
|
|
{
|
|
int32 i1 = i;
|
|
int32 i2 = i + 1 < m_count ? 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();
|
|
}
|
|
|
|
// Compute the polygon centroid.
|
|
m_centroid = ComputeCentroid(m_vertices, m_count);
|
|
}
|
|
|
|
bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
|
|
{
|
|
b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
|
|
|
|
for (int32 i = 0; i < m_count; ++i)
|
|
{
|
|
float 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.q, input.p1 - xf.p);
|
|
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
|
|
b2Vec2 d = p2 - p1;
|
|
|
|
float lower = 0.0f, upper = input.maxFraction;
|
|
|
|
int32 index = -1;
|
|
|
|
for (int32 i = 0; i < m_count; ++i)
|
|
{
|
|
// p = p1 + a * d
|
|
// dot(normal, p - v) = 0
|
|
// dot(normal, p1 - v) + a * dot(normal, d) = 0
|
|
float numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
|
|
float 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.q, 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_count; ++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, float 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_count >= 3);
|
|
|
|
b2Vec2 center(0.0f, 0.0f);
|
|
float area = 0.0f;
|
|
float I = 0.0f;
|
|
|
|
// Get a reference point for forming triangles.
|
|
// Use the first vertex to reduce round-off errors.
|
|
b2Vec2 s = m_vertices[0];
|
|
|
|
const float k_inv3 = 1.0f / 3.0f;
|
|
|
|
for (int32 i = 0; i < m_count; ++i)
|
|
{
|
|
// Triangle vertices.
|
|
b2Vec2 e1 = m_vertices[i] - s;
|
|
b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
|
|
|
|
float D = b2Cross(e1, e2);
|
|
|
|
float triangleArea = 0.5f * D;
|
|
area += triangleArea;
|
|
|
|
// Area weighted centroid
|
|
center += triangleArea * k_inv3 * (e1 + e2);
|
|
|
|
float ex1 = e1.x, ey1 = e1.y;
|
|
float ex2 = e2.x, ey2 = e2.y;
|
|
|
|
float intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
|
|
float inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
|
|
|
|
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
|
|
}
|
|
|
|
// Total mass
|
|
massData->mass = density * area;
|
|
|
|
// Center of mass
|
|
b2Assert(area > b2_epsilon);
|
|
center *= 1.0f / area;
|
|
massData->center = center + s;
|
|
|
|
// Inertia tensor relative to the local origin (point s).
|
|
massData->I = density * I;
|
|
|
|
// Shift to center of mass then to original body origin.
|
|
massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
|
|
}
|
|
|
|
bool b2PolygonShape::Validate() const
|
|
{
|
|
if (m_count < 3 || b2_maxPolygonVertices < m_count)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
b2Hull hull;
|
|
for (int32 i = 0; i < m_count; ++i)
|
|
{
|
|
hull.points[i] = m_vertices[i];
|
|
}
|
|
|
|
hull.count = m_count;
|
|
|
|
return b2ValidateHull(hull);
|
|
}
|