《实时碰撞检测算法技术》源代码

=== Section 5.1.1: ============================================================= Point ClosestPtPointPlane(Point q, Plane p) { float t = (Dot(p.n, q) – p.d) / Dot(p.n, p.n); return q – t * p.n; } -------------------------------------------------------------------------------- Point ClosestPtPointPlane(Point q, Plane p) { float t = Dot(p.n, q) – p.d; return q – t * p.n; } -------------------------------------------------------------------------------- float DistPointPlane(Point q, Plane p) { // return Dot(q, p.n) – p.d; if plane equation normalized (||p.n||==1) return (Dot(p.n, q) – p.d) / Dot(p.n, p.n); } === Section 5.1.2: ============================================================= // Given segment ab and point c, computes closest point d on ab. // Also returns t for the position of d, d(t) = a + t*(b - a) void ClosestPtPointSegment(Point c, Point a, Point b, float &t, Point &d) { Vector ab = b – a; // Project c onto ab, computing parameterized position d(t) = a + t*(b – a) t = Dot(c – a, ab) / Dot(ab, ab); // If outside segment, clamp t (and therefore d) to the closest endpoint if (t < 0.0f) t = 0.0f; if (t > 1.0f) t = 1.0f; // Compute projected position from the clamped t d = a + t * ab; } -------------------------------------------------------------------------------- // Given segment ab and point c, computes closest point d on ab. // Also returns t for the parametric position of d, d(t) = a + t*(b - a) void ClosestPtPointSegment(Point c, Point a, Point b, float &t, Point &d) { Vector ab = b – a; // Project c onto ab, but deferring divide by Dot(ab, ab) t = Dot(c – a, ab); if (t <= 0.0f) { // c projects outside the [a,b] interval, on the a side; clamp to a t = 0.0f; d = a; } else { float denom = Dot(ab, ab); // Always nonnegative since denom = ||ab||^2 if (t >= denom) { // c projects outside the [a,b] interval, on the b side; clamp to b t = 1.0f; d = b; } else { // c projects inside the [a,b] interval; must do deferred divide now t = t / denom; d = a + t * ab; } } } === Section 5.1.2.1: =========================================================== // Returns the squared distance between point c and segment ab float SqDistPointSegment(Point a, Point b, Point c) { Vector ab = b – a, ac = c – a, bc = c – b; float e = Dot(ac, ab); // Handle cases where c projects outside ab if (e <= 0.0f) return Dot(ac, ac); float f = Dot(ab, ab); if (e >= f) return Dot(bc, bc); // Handle case where c projects onto ab return Dot(ac, ac) – e * e / f; } === Section 5.1.3: ============================================================= // Given point p, return the point q on or in AABB b, that is closest to p void ClosestPtPointAABB(Point p, AABB b, Point &q) { // For each coordinate axis, if the point coordinate value is // outside box, clamp it to the box, else keep it as is for (int i = 0; i < 3; i++) { float v = p[i]; if (v < b.min[i]) v = b.min[i]; // v = max(v, b.min[i]) if (v > b.max[i]) v = b.max[i]; // v = min(v, b.max[i]) q[i] = v; } } === Section 5.1.3.1: =========================================================== // Computes the square distance between a point p and an AABB b float SqDistPointAABB(Point p, AABB b) { float sqDist = 0.0f; for (int i = 0; i < 3; i++) { // For each axis count any excess distance outside box extents float v = p[i]; if (v < b.min[i]) sqDist += (b.min[i] – v) * (b.min[i] – v); if (v > b.max[i]) sqDist += (v – b.max[i]) * (v – b.max[i]); } return sqDist; } === Section 5.1.4: ============================================================= // Given point p, return point q on (or in) OBB b, closest to p void ClosestPtPointOBB(Point p, OBB b, Point &q) { Vector d = p – b.c; // Start result at center of box; make steps from there q = b.c; // For each OBB axis... for (int i = 0; i < 3; i++) { // ...project d onto that axis to get the distance // along the axis of d from the box center float dist = Dot(d, b.u[i]); // If distance farther than the box extents, clamp to the box if (dist > b.e[i]) dist = b.e[i]; if (dist < –b.e[i]) dist = –b.e[i]; // Step that distance along the axis to get world coordinate q += dist * b.u[i]; } } === Section 5.1.4.1: =========================================================== // Computes the square distance between point p and OBB b float SqDistPointOBB(Point p, OBB b) { Point closest; ClosestPtPointOBB(p, b, closest); float sqDist = Dot(closest – p, closest – p); return sqDist; } -------------------------------------------------------------------------------- // Computes the square distance between point p and OBB b float SqDistPointOBB(Point p, OBB b) { Vector v = p – b.c; float sqDist = 0.0f; for (int i = 0; i < 3; i++) { // Project vector from box center to p on each axis, getting the distance // of p along that axis, and count any excess distance outside box extents float d = Dot(v, b.u[i]), excess = 0.0f; if (d < –b.e[i]) excess = d + b.e[i]; else if (d > b.e[i]) excess = d – b.e[i]; sqDist += excess * excess; } return sqDist; } === Section 5.1.4.2: =========================================================== struct Rect { Point c; // center point of rectangle Vector u[2]; // unit vectors determining local x and y axes for the rectangle float e[2]; // the halfwidth extents of the rectangle along the axes }; -------------------------------------------------------------------------------- // Given point p, return point q on (or in) Rect r, closest to p void ClosestPtPointRect(Point p, Rect r, Point &q) { Vector d = p – r.c; // Start result at center of rect; make steps from there q = r.c; // For each rect axis... for (int i = 0; i < 2; i++) { // ...project d onto that axis to get the distance // along the axis of d from the rect center float dist = Dot(d, r.u[i]); // If distance farther than the rect extents, clamp to the rect if (dist > r.e[i]) dist = r.e[i]; if (dist < –r.e[i]) dist = –r.e[i]; // Step that distance along the axis to get world coordinate q += dist * r.u[i]; } } -------------------------------------------------------------------------------- // Return point q on (or in) rect (specified by a, b, and c), closest to given point p void ClosestPtPointRect(Point p, Point a, Point b, Point c, Point &q) { Vector ab = b – a; // vector across rect Vector ac = c – a; // vector down rect Vector d = p – a; // Start result at top-left corner of rect; make steps from there q = a; // Clamp p’ (projection of p to plane of r) to rectangle in the across direction float dist = Dot(d, ab); float maxdist = Dot(ab, ab); if (dist >= maxdist) q += ab; else if (dist > 0.0f) q += (dist / maxdist) * ab; // Clamp p’ (projection of p to plane of r) to rectangle in the down direction dist = Dot(d, ac); maxdist = Dot(ac, ac); if (dist >= maxdist) q += ac; else if (dist > 0.0f) q += (dist / maxdist) * ac; } === Section 5.1.5: ============================================================= Vector n = Cross(b – a, c – a); float rab = Dot(n, Cross(a – r, b – r)); // proportional to signed area of RAB float rbc = Dot(n, Cross(b – r, c – r)); // proportional to signed