Warning, file /education/gcompris/external/qml-box2d/Box2D/Collision/b2Collision.cpp was not indexed or was modified since last indexation (in which case cross-reference links may be missing, inaccurate or erroneous).
0001 /* 0002 * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Collision/b2Collision.h> 0020 #include <Box2D/Collision/b2Distance.h> 0021 0022 void b2WorldManifold::Initialize(const b2Manifold* manifold, 0023 const b2Transform& xfA, float32 radiusA, 0024 const b2Transform& xfB, float32 radiusB) 0025 { 0026 if (manifold->pointCount == 0) 0027 { 0028 return; 0029 } 0030 0031 switch (manifold->type) 0032 { 0033 case b2Manifold::e_circles: 0034 { 0035 normal.Set(1.0f, 0.0f); 0036 b2Vec2 pointA = b2Mul(xfA, manifold->localPoint); 0037 b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint); 0038 if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) 0039 { 0040 normal = pointB - pointA; 0041 normal.Normalize(); 0042 } 0043 0044 b2Vec2 cA = pointA + radiusA * normal; 0045 b2Vec2 cB = pointB - radiusB * normal; 0046 points[0] = 0.5f * (cA + cB); 0047 separations[0] = b2Dot(cB - cA, normal); 0048 } 0049 break; 0050 0051 case b2Manifold::e_faceA: 0052 { 0053 normal = b2Mul(xfA.q, manifold->localNormal); 0054 b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); 0055 0056 for (int32 i = 0; i < manifold->pointCount; ++i) 0057 { 0058 b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); 0059 b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; 0060 b2Vec2 cB = clipPoint - radiusB * normal; 0061 points[i] = 0.5f * (cA + cB); 0062 separations[i] = b2Dot(cB - cA, normal); 0063 } 0064 } 0065 break; 0066 0067 case b2Manifold::e_faceB: 0068 { 0069 normal = b2Mul(xfB.q, manifold->localNormal); 0070 b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); 0071 0072 for (int32 i = 0; i < manifold->pointCount; ++i) 0073 { 0074 b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); 0075 b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; 0076 b2Vec2 cA = clipPoint - radiusA * normal; 0077 points[i] = 0.5f * (cA + cB); 0078 separations[i] = b2Dot(cA - cB, normal); 0079 } 0080 0081 // Ensure normal points from A to B. 0082 normal = -normal; 0083 } 0084 break; 0085 } 0086 } 0087 0088 void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], 0089 const b2Manifold* manifold1, const b2Manifold* manifold2) 0090 { 0091 for (int32 i = 0; i < b2_maxManifoldPoints; ++i) 0092 { 0093 state1[i] = b2_nullState; 0094 state2[i] = b2_nullState; 0095 } 0096 0097 // Detect persists and removes. 0098 for (int32 i = 0; i < manifold1->pointCount; ++i) 0099 { 0100 b2ContactID id = manifold1->points[i].id; 0101 0102 state1[i] = b2_removeState; 0103 0104 for (int32 j = 0; j < manifold2->pointCount; ++j) 0105 { 0106 if (manifold2->points[j].id.key == id.key) 0107 { 0108 state1[i] = b2_persistState; 0109 break; 0110 } 0111 } 0112 } 0113 0114 // Detect persists and adds. 0115 for (int32 i = 0; i < manifold2->pointCount; ++i) 0116 { 0117 b2ContactID id = manifold2->points[i].id; 0118 0119 state2[i] = b2_addState; 0120 0121 for (int32 j = 0; j < manifold1->pointCount; ++j) 0122 { 0123 if (manifold1->points[j].id.key == id.key) 0124 { 0125 state2[i] = b2_persistState; 0126 break; 0127 } 0128 } 0129 } 0130 } 0131 0132 // From Real-time Collision Detection, p179. 0133 bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const 0134 { 0135 float32 tmin = -b2_maxFloat; 0136 float32 tmax = b2_maxFloat; 0137 0138 b2Vec2 p = input.p1; 0139 b2Vec2 d = input.p2 - input.p1; 0140 b2Vec2 absD = b2Abs(d); 0141 0142 b2Vec2 normal; 0143 0144 for (int32 i = 0; i < 2; ++i) 0145 { 0146 if (absD(i) < b2_epsilon) 0147 { 0148 // Parallel. 0149 if (p(i) < lowerBound(i) || upperBound(i) < p(i)) 0150 { 0151 return false; 0152 } 0153 } 0154 else 0155 { 0156 float32 inv_d = 1.0f / d(i); 0157 float32 t1 = (lowerBound(i) - p(i)) * inv_d; 0158 float32 t2 = (upperBound(i) - p(i)) * inv_d; 0159 0160 // Sign of the normal vector. 0161 float32 s = -1.0f; 0162 0163 if (t1 > t2) 0164 { 0165 b2Swap(t1, t2); 0166 s = 1.0f; 0167 } 0168 0169 // Push the min up 0170 if (t1 > tmin) 0171 { 0172 normal.SetZero(); 0173 normal(i) = s; 0174 tmin = t1; 0175 } 0176 0177 // Pull the max down 0178 tmax = b2Min(tmax, t2); 0179 0180 if (tmin > tmax) 0181 { 0182 return false; 0183 } 0184 } 0185 } 0186 0187 // Does the ray start inside the box? 0188 // Does the ray intersect beyond the max fraction? 0189 if (tmin < 0.0f || input.maxFraction < tmin) 0190 { 0191 return false; 0192 } 0193 0194 // Intersection. 0195 output->fraction = tmin; 0196 output->normal = normal; 0197 return true; 0198 } 0199 0200 // Sutherland-Hodgman clipping. 0201 int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], 0202 const b2Vec2& normal, float32 offset, int32 vertexIndexA) 0203 { 0204 // Start with no output points 0205 int32 numOut = 0; 0206 0207 // Calculate the distance of end points to the line 0208 float32 distance0 = b2Dot(normal, vIn[0].v) - offset; 0209 float32 distance1 = b2Dot(normal, vIn[1].v) - offset; 0210 0211 // If the points are behind the plane 0212 if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; 0213 if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; 0214 0215 // If the points are on different sides of the plane 0216 if (distance0 * distance1 < 0.0f) 0217 { 0218 // Find intersection point of edge and plane 0219 float32 interp = distance0 / (distance0 - distance1); 0220 vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); 0221 0222 // VertexA is hitting edgeB. 0223 vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA); 0224 vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB; 0225 vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex; 0226 vOut[numOut].id.cf.typeB = b2ContactFeature::e_face; 0227 ++numOut; 0228 } 0229 0230 return numOut; 0231 } 0232 0233 bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, 0234 const b2Shape* shapeB, int32 indexB, 0235 const b2Transform& xfA, const b2Transform& xfB) 0236 { 0237 b2DistanceInput input; 0238 input.proxyA.Set(shapeA, indexA); 0239 input.proxyB.Set(shapeB, indexB); 0240 input.transformA = xfA; 0241 input.transformB = xfB; 0242 input.useRadii = true; 0243 0244 b2SimplexCache cache; 0245 cache.count = 0; 0246 0247 b2DistanceOutput output; 0248 0249 b2Distance(&output, &cache, &input); 0250 0251 return output.distance < 10.0f * b2_epsilon; 0252 }