b2Collision.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2007-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/b2Collision.h>
00020 #include <Box2D/Collision/b2Distance.h>
00021 
00022 void b2WorldManifold::Initialize(const b2Manifold* manifold,
00023                                                   const b2Transform& xfA, float32 radiusA,
00024                                                   const b2Transform& xfB, float32 radiusB)
00025 {
00026         if (manifold->pointCount == 0)
00027         {
00028                 return;
00029         }
00030 
00031         switch (manifold->type)
00032         {
00033         case b2Manifold::e_circles:
00034                 {
00035                         normal.Set(1.0f, 0.0f);
00036                         b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
00037                         b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
00038                         if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
00039                         {
00040                                 normal = pointB - pointA;
00041                                 normal.Normalize();
00042                         }
00043 
00044                         b2Vec2 cA = pointA + radiusA * normal;
00045                         b2Vec2 cB = pointB - radiusB * normal;
00046                         points[0] = 0.5f * (cA + cB);
00047                         separations[0] = b2Dot(cB - cA, normal);
00048                 }
00049                 break;
00050 
00051         case b2Manifold::e_faceA:
00052                 {
00053                         normal = b2Mul(xfA.q, manifold->localNormal);
00054                         b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
00055                         
00056                         for (int32 i = 0; i < manifold->pointCount; ++i)
00057                         {
00058                                 b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
00059                                 b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
00060                                 b2Vec2 cB = clipPoint - radiusB * normal;
00061                                 points[i] = 0.5f * (cA + cB);
00062                                 separations[i] = b2Dot(cB - cA, normal);
00063                         }
00064                 }
00065                 break;
00066 
00067         case b2Manifold::e_faceB:
00068                 {
00069                         normal = b2Mul(xfB.q, manifold->localNormal);
00070                         b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
00071 
00072                         for (int32 i = 0; i < manifold->pointCount; ++i)
00073                         {
00074                                 b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
00075                                 b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
00076                                 b2Vec2 cA = clipPoint - radiusA * normal;
00077                                 points[i] = 0.5f * (cA + cB);
00078                                 separations[i] = b2Dot(cA - cB, normal);
00079                         }
00080 
00081                         // Ensure normal points from A to B.
00082                         normal = -normal;
00083                 }
00084                 break;
00085         }
00086 }
00087 
00088 void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
00089                                           const b2Manifold* manifold1, const b2Manifold* manifold2)
00090 {
00091         for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
00092         {
00093                 state1[i] = b2_nullState;
00094                 state2[i] = b2_nullState;
00095         }
00096 
00097         // Detect persists and removes.
00098         for (int32 i = 0; i < manifold1->pointCount; ++i)
00099         {
00100                 b2ContactID id = manifold1->points[i].id;
00101 
00102                 state1[i] = b2_removeState;
00103 
00104                 for (int32 j = 0; j < manifold2->pointCount; ++j)
00105                 {
00106                         if (manifold2->points[j].id.key == id.key)
00107                         {
00108                                 state1[i] = b2_persistState;
00109                                 break;
00110                         }
00111                 }
00112         }
00113 
00114         // Detect persists and adds.
00115         for (int32 i = 0; i < manifold2->pointCount; ++i)
00116         {
00117                 b2ContactID id = manifold2->points[i].id;
00118 
00119                 state2[i] = b2_addState;
00120 
00121                 for (int32 j = 0; j < manifold1->pointCount; ++j)
00122                 {
00123                         if (manifold1->points[j].id.key == id.key)
00124                         {
00125                                 state2[i] = b2_persistState;
00126                                 break;
00127                         }
00128                 }
00129         }
00130 }
00131 
00132 // From Real-time Collision Detection, p179.
00133 bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
00134 {
00135         float32 tmin = -b2_maxFloat;
00136         float32 tmax = b2_maxFloat;
00137 
00138         b2Vec2 p = input.p1;
00139         b2Vec2 d = input.p2 - input.p1;
00140         b2Vec2 absD = b2Abs(d);
00141 
00142         b2Vec2 normal;
00143 
00144         for (int32 i = 0; i < 2; ++i)
00145         {
00146                 if (absD(i) < b2_epsilon)
00147                 {
00148                         // Parallel.
00149                         if (p(i) < lowerBound(i) || upperBound(i) < p(i))
00150                         {
00151                                 return false;
00152                         }
00153                 }
00154                 else
00155                 {
00156                         float32 inv_d = 1.0f / d(i);
00157                         float32 t1 = (lowerBound(i) - p(i)) * inv_d;
00158                         float32 t2 = (upperBound(i) - p(i)) * inv_d;
00159 
00160                         // Sign of the normal vector.
00161                         float32 s = -1.0f;
00162 
00163                         if (t1 > t2)
00164                         {
00165                                 b2Swap(t1, t2);
00166                                 s = 1.0f;
00167                         }
00168 
00169                         // Push the min up
00170                         if (t1 > tmin)
00171                         {
00172                                 normal.SetZero();
00173                                 normal(i) = s;
00174                                 tmin = t1;
00175                         }
00176 
00177                         // Pull the max down
00178                         tmax = b2Min(tmax, t2);
00179 
00180                         if (tmin > tmax)
00181                         {
00182                                 return false;
00183                         }
00184                 }
00185         }
00186 
00187         // Does the ray start inside the box?
00188         // Does the ray intersect beyond the max fraction?
00189         if (tmin < 0.0f || input.maxFraction < tmin)
00190         {
00191                 return false;
00192         }
00193 
00194         // Intersection.
00195         output->fraction = tmin;
00196         output->normal = normal;
00197         return true;
00198 }
00199 
00200 // Sutherland-Hodgman clipping.
00201 int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
00202                                                 const b2Vec2& normal, float32 offset, int32 vertexIndexA)
00203 {
00204         // Start with no output points
00205         int32 numOut = 0;
00206 
00207         // Calculate the distance of end points to the line
00208         float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
00209         float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
00210 
00211         // If the points are behind the plane
00212         if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
00213         if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
00214 
00215         // If the points are on different sides of the plane
00216         if (distance0 * distance1 < 0.0f)
00217         {
00218                 // Find intersection point of edge and plane
00219                 float32 interp = distance0 / (distance0 - distance1);
00220                 vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
00221 
00222                 // VertexA is hitting edgeB.
00223                 vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);
00224                 vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
00225                 vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
00226                 vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;
00227                 ++numOut;
00228         }
00229 
00230         return numOut;
00231 }
00232 
00233 bool b2TestOverlap(     const b2Shape* shapeA, int32 indexA,
00234                                         const b2Shape* shapeB, int32 indexB,
00235                                         const b2Transform& xfA, const b2Transform& xfB)
00236 {
00237         b2DistanceInput input;
00238         input.proxyA.Set(shapeA, indexA);
00239         input.proxyB.Set(shapeB, indexB);
00240         input.transformA = xfA;
00241         input.transformB = xfB;
00242         input.useRadii = true;
00243 
00244         b2SimplexCache cache;
00245         cache.count = 0;
00246 
00247         b2DistanceOutput output;
00248 
00249         b2Distance(&output, &cache, &input);
00250 
00251         return output.distance < 10.0f * b2_epsilon;
00252 }


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