b2PolygonShape.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
00020 #include <new>
00021 
00022 b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
00023 {
00024         void* mem = allocator->Allocate(sizeof(b2PolygonShape));
00025         b2PolygonShape* clone = new (mem) b2PolygonShape;
00026         *clone = *this;
00027         return clone;
00028 }
00029 
00030 void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
00031 {
00032         m_count = 4;
00033         m_vertices[0].Set(-hx, -hy);
00034         m_vertices[1].Set( hx, -hy);
00035         m_vertices[2].Set( hx,  hy);
00036         m_vertices[3].Set(-hx,  hy);
00037         m_normals[0].Set(0.0f, -1.0f);
00038         m_normals[1].Set(1.0f, 0.0f);
00039         m_normals[2].Set(0.0f, 1.0f);
00040         m_normals[3].Set(-1.0f, 0.0f);
00041         m_centroid.SetZero();
00042 }
00043 
00044 void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
00045 {
00046         m_count = 4;
00047         m_vertices[0].Set(-hx, -hy);
00048         m_vertices[1].Set( hx, -hy);
00049         m_vertices[2].Set( hx,  hy);
00050         m_vertices[3].Set(-hx,  hy);
00051         m_normals[0].Set(0.0f, -1.0f);
00052         m_normals[1].Set(1.0f, 0.0f);
00053         m_normals[2].Set(0.0f, 1.0f);
00054         m_normals[3].Set(-1.0f, 0.0f);
00055         m_centroid = center;
00056 
00057         b2Transform xf;
00058         xf.p = center;
00059         xf.q.Set(angle);
00060 
00061         // Transform vertices and normals.
00062         for (int32 i = 0; i < m_count; ++i)
00063         {
00064                 m_vertices[i] = b2Mul(xf, m_vertices[i]);
00065                 m_normals[i] = b2Mul(xf.q, m_normals[i]);
00066         }
00067 }
00068 
00069 int32 b2PolygonShape::GetChildCount() const
00070 {
00071         return 1;
00072 }
00073 
00074 static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
00075 {
00076         b2Assert(count >= 3);
00077 
00078         b2Vec2 c; c.Set(0.0f, 0.0f);
00079         float32 area = 0.0f;
00080 
00081         // pRef is the reference point for forming triangles.
00082         // It's location doesn't change the result (except for rounding error).
00083         b2Vec2 pRef(0.0f, 0.0f);
00084 #if 0
00085         // This code would put the reference point inside the polygon.
00086         for (int32 i = 0; i < count; ++i)
00087         {
00088                 pRef += vs[i];
00089         }
00090         pRef *= 1.0f / count;
00091 #endif
00092 
00093         const float32 inv3 = 1.0f / 3.0f;
00094 
00095         for (int32 i = 0; i < count; ++i)
00096         {
00097                 // Triangle vertices.
00098                 b2Vec2 p1 = pRef;
00099                 b2Vec2 p2 = vs[i];
00100                 b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
00101 
00102                 b2Vec2 e1 = p2 - p1;
00103                 b2Vec2 e2 = p3 - p1;
00104 
00105                 float32 D = b2Cross(e1, e2);
00106 
00107                 float32 triangleArea = 0.5f * D;
00108                 area += triangleArea;
00109 
00110                 // Area weighted centroid
00111                 c += triangleArea * inv3 * (p1 + p2 + p3);
00112         }
00113 
00114         // Centroid
00115         b2Assert(area > b2_epsilon);
00116         c *= 1.0f / area;
00117         return c;
00118 }
00119 
00120 void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
00121 {
00122         b2Assert(3 <= count && count <= b2_maxPolygonVertices);
00123         if (count < 3)
00124         {
00125                 SetAsBox(1.0f, 1.0f);
00126                 return;
00127         }
00128         
00129         int32 n = b2Min(count, b2_maxPolygonVertices);
00130 
00131         // Perform welding and copy vertices into local buffer.
00132         b2Vec2 ps[b2_maxPolygonVertices];
00133         int32 tempCount = 0;
00134         for (int32 i = 0; i < n; ++i)
00135         {
00136                 b2Vec2 v = vertices[i];
00137 
00138                 bool unique = true;
00139                 for (int32 j = 0; j < tempCount; ++j)
00140                 {
00141                         if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop)
00142                         {
00143                                 unique = false;
00144                                 break;
00145                         }
00146                 }
00147 
00148                 if (unique)
00149                 {
00150                         ps[tempCount++] = v;
00151                 }
00152         }
00153 
00154         n = tempCount;
00155         if (n < 3)
00156         {
00157                 // Polygon is degenerate.
00158                 b2Assert(false);
00159                 SetAsBox(1.0f, 1.0f);
00160                 return;
00161         }
00162 
00163         // Create the convex hull using the Gift wrapping algorithm
00164         // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
00165 
00166         // Find the right most point on the hull
00167         int32 i0 = 0;
00168         float32 x0 = ps[0].x;
00169         for (int32 i = 1; i < n; ++i)
00170         {
00171                 float32 x = ps[i].x;
00172                 if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
00173                 {
00174                         i0 = i;
00175                         x0 = x;
00176                 }
00177         }
00178 
00179         int32 hull[b2_maxPolygonVertices];
00180         int32 m = 0;
00181         int32 ih = i0;
00182 
00183         for (;;)
00184         {
00185                 hull[m] = ih;
00186 
00187                 int32 ie = 0;
00188                 for (int32 j = 1; j < n; ++j)
00189                 {
00190                         if (ie == ih)
00191                         {
00192                                 ie = j;
00193                                 continue;
00194                         }
00195 
00196                         b2Vec2 r = ps[ie] - ps[hull[m]];
00197                         b2Vec2 v = ps[j] - ps[hull[m]];
00198                         float32 c = b2Cross(r, v);
00199                         if (c < 0.0f)
00200                         {
00201                                 ie = j;
00202                         }
00203 
00204                         // Collinearity check
00205                         if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
00206                         {
00207                                 ie = j;
00208                         }
00209                 }
00210 
00211                 ++m;
00212                 ih = ie;
00213 
00214                 if (ie == i0)
00215                 {
00216                         break;
00217                 }
00218         }
00219         
00220         if (m < 3)
00221         {
00222                 // Polygon is degenerate.
00223                 b2Assert(false);
00224                 SetAsBox(1.0f, 1.0f);
00225                 return;
00226         }
00227 
00228         m_count = m;
00229 
00230         // Copy vertices.
00231         for (int32 i = 0; i < m; ++i)
00232         {
00233                 m_vertices[i] = ps[hull[i]];
00234         }
00235 
00236         // Compute normals. Ensure the edges have non-zero length.
00237         for (int32 i = 0; i < m; ++i)
00238         {
00239                 int32 i1 = i;
00240                 int32 i2 = i + 1 < m ? i + 1 : 0;
00241                 b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
00242                 b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
00243                 m_normals[i] = b2Cross(edge, 1.0f);
00244                 m_normals[i].Normalize();
00245         }
00246 
00247         // Compute the polygon centroid.
00248         m_centroid = ComputeCentroid(m_vertices, m);
00249 }
00250 
00251 bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
00252 {
00253         b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
00254 
00255         for (int32 i = 0; i < m_count; ++i)
00256         {
00257                 float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
00258                 if (dot > 0.0f)
00259                 {
00260                         return false;
00261                 }
00262         }
00263 
00264         return true;
00265 }
00266 
00267 bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
00268                                                                 const b2Transform& xf, int32 childIndex) const
00269 {
00270         B2_NOT_USED(childIndex);
00271 
00272         // Put the ray into the polygon's frame of reference.
00273         b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
00274         b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
00275         b2Vec2 d = p2 - p1;
00276 
00277         float32 lower = 0.0f, upper = input.maxFraction;
00278 
00279         int32 index = -1;
00280 
00281         for (int32 i = 0; i < m_count; ++i)
00282         {
00283                 // p = p1 + a * d
00284                 // dot(normal, p - v) = 0
00285                 // dot(normal, p1 - v) + a * dot(normal, d) = 0
00286                 float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
00287                 float32 denominator = b2Dot(m_normals[i], d);
00288 
00289                 if (denominator == 0.0f)
00290                 {       
00291                         if (numerator < 0.0f)
00292                         {
00293                                 return false;
00294                         }
00295                 }
00296                 else
00297                 {
00298                         // Note: we want this predicate without division:
00299                         // lower < numerator / denominator, where denominator < 0
00300                         // Since denominator < 0, we have to flip the inequality:
00301                         // lower < numerator / denominator <==> denominator * lower > numerator.
00302                         if (denominator < 0.0f && numerator < lower * denominator)
00303                         {
00304                                 // Increase lower.
00305                                 // The segment enters this half-space.
00306                                 lower = numerator / denominator;
00307                                 index = i;
00308                         }
00309                         else if (denominator > 0.0f && numerator < upper * denominator)
00310                         {
00311                                 // Decrease upper.
00312                                 // The segment exits this half-space.
00313                                 upper = numerator / denominator;
00314                         }
00315                 }
00316 
00317                 // The use of epsilon here causes the assert on lower to trip
00318                 // in some cases. Apparently the use of epsilon was to make edge
00319                 // shapes work, but now those are handled separately.
00320                 //if (upper < lower - b2_epsilon)
00321                 if (upper < lower)
00322                 {
00323                         return false;
00324                 }
00325         }
00326 
00327         b2Assert(0.0f <= lower && lower <= input.maxFraction);
00328 
00329         if (index >= 0)
00330         {
00331                 output->fraction = lower;
00332                 output->normal = b2Mul(xf.q, m_normals[index]);
00333                 return true;
00334         }
00335 
00336         return false;
00337 }
00338 
00339 void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
00340 {
00341         B2_NOT_USED(childIndex);
00342 
00343         b2Vec2 lower = b2Mul(xf, m_vertices[0]);
00344         b2Vec2 upper = lower;
00345 
00346         for (int32 i = 1; i < m_count; ++i)
00347         {
00348                 b2Vec2 v = b2Mul(xf, m_vertices[i]);
00349                 lower = b2Min(lower, v);
00350                 upper = b2Max(upper, v);
00351         }
00352 
00353         b2Vec2 r(m_radius, m_radius);
00354         aabb->lowerBound = lower - r;
00355         aabb->upperBound = upper + r;
00356 }
00357 
00358 void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
00359 {
00360         // Polygon mass, centroid, and inertia.
00361         // Let rho be the polygon density in mass per unit area.
00362         // Then:
00363         // mass = rho * int(dA)
00364         // centroid.x = (1/mass) * rho * int(x * dA)
00365         // centroid.y = (1/mass) * rho * int(y * dA)
00366         // I = rho * int((x*x + y*y) * dA)
00367         //
00368         // We can compute these integrals by summing all the integrals
00369         // for each triangle of the polygon. To evaluate the integral
00370         // for a single triangle, we make a change of variables to
00371         // the (u,v) coordinates of the triangle:
00372         // x = x0 + e1x * u + e2x * v
00373         // y = y0 + e1y * u + e2y * v
00374         // where 0 <= u && 0 <= v && u + v <= 1.
00375         //
00376         // We integrate u from [0,1-v] and then v from [0,1].
00377         // We also need to use the Jacobian of the transformation:
00378         // D = cross(e1, e2)
00379         //
00380         // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
00381         //
00382         // The rest of the derivation is handled by computer algebra.
00383 
00384         b2Assert(m_count >= 3);
00385 
00386         b2Vec2 center; center.Set(0.0f, 0.0f);
00387         float32 area = 0.0f;
00388         float32 I = 0.0f;
00389 
00390         // s is the reference point for forming triangles.
00391         // It's location doesn't change the result (except for rounding error).
00392         b2Vec2 s(0.0f, 0.0f);
00393 
00394         // This code would put the reference point inside the polygon.
00395         for (int32 i = 0; i < m_count; ++i)
00396         {
00397                 s += m_vertices[i];
00398         }
00399         s *= 1.0f / m_count;
00400 
00401         const float32 k_inv3 = 1.0f / 3.0f;
00402 
00403         for (int32 i = 0; i < m_count; ++i)
00404         {
00405                 // Triangle vertices.
00406                 b2Vec2 e1 = m_vertices[i] - s;
00407                 b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
00408 
00409                 float32 D = b2Cross(e1, e2);
00410 
00411                 float32 triangleArea = 0.5f * D;
00412                 area += triangleArea;
00413 
00414                 // Area weighted centroid
00415                 center += triangleArea * k_inv3 * (e1 + e2);
00416 
00417                 float32 ex1 = e1.x, ey1 = e1.y;
00418                 float32 ex2 = e2.x, ey2 = e2.y;
00419 
00420                 float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
00421                 float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
00422 
00423                 I += (0.25f * k_inv3 * D) * (intx2 + inty2);
00424         }
00425 
00426         // Total mass
00427         massData->mass = density * area;
00428 
00429         // Center of mass
00430         b2Assert(area > b2_epsilon);
00431         center *= 1.0f / area;
00432         massData->center = center + s;
00433 
00434         // Inertia tensor relative to the local origin (point s).
00435         massData->I = density * I;
00436         
00437         // Shift to center of mass then to original body origin.
00438         massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
00439 }
00440 
00441 bool b2PolygonShape::Validate() const
00442 {
00443         for (int32 i = 0; i < m_count; ++i)
00444         {
00445                 int32 i1 = i;
00446                 int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
00447                 b2Vec2 p = m_vertices[i1];
00448                 b2Vec2 e = m_vertices[i2] - p;
00449 
00450                 for (int32 j = 0; j < m_count; ++j)
00451                 {
00452                         if (j == i1 || j == i2)
00453                         {
00454                                 continue;
00455                         }
00456 
00457                         b2Vec2 v = m_vertices[j] - p;
00458                         float32 c = b2Cross(e, v);
00459                         if (c < 0.0f)
00460                         {
00461                                 return false;
00462                         }
00463                 }
00464         }
00465 
00466         return true;
00467 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:34