이 분석은 Erin Catto의 GDC 2006~7 발표와, Randy Gaul의 자료, 그리고 Erin의 다른 포럼에서의 설명등을 기반으로 한다.
Collide.cpp 파일에는 세 개의 주된 함수가 존재한다.
int Collide(Contact* contacts, Body* bodyA, Body* bodyB); static void ComputeIncidentEdge(ClipVertex c[2], const Vec2& h, const Vec2& pos, const Mat22& Rot, const Vec2& normal); int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], const Vec2& normal, float offset, char clipEdge);
그리고 다음과 같은 구조체들과 보조 함수 하나가 쓰인다
enum Axis { FACE_A_X, FACE_A_Y, FACE_B_X, FACE_B_Y }; enum EdgeNumbers { NO_EDGE = 0, EDGE1, EDGE2, EDGE3, EDGE4 }; struct ClipVertex { ClipVertex() { fp.value = 0; } Vec2 v; FeaturePair fp; }; void Flip(FeaturePair& fp) { Swap(fp.e.inEdge1, fp.e.inEdge2); Swap(fp.e.outEdge1, fp.e.outEdge2); }
Collide함수는 2D의 Box가 충돌하는지를 보고, Contacts 관련 정보를 생성한다.
이 때, ComputeIncidentEdge함수와 ClipSegmentToLine 함수를 이용한다.
진행하기전에 알야아 할 것은, Box의 Vertex와 Edge의 숫자가 부여되는 순서이다. 이것은 Axis와 EdgeNumbers enum과 관련이 있다.
 |
| Boix2D Collide.cpp |
파일에 vertex와 edge의 numbering이 설명되어있다. 이것은 중요한 부분이므로, 자주 참고하게 된다.
이제 Collide함수에 들어가보자, 코드 분석은 의미단위로 나눠서 분석한다.
1. 충돌 준비하기 (Collision Setup)
// The normal points from A to B int Collide(Contact* contacts, Body* bodyA, Body* bodyB) { // Setup Vec2 hA = 0.5f * bodyA->width; Vec2 hB = 0.5f * bodyB->width; Vec2 posA = bodyA->position; Vec2 posB = bodyB->position; Mat22 RotA(bodyA->rotation), RotB(bodyB->rotation); Mat22 RotAT = RotA.Transpose(); Mat22 RotBT = RotB.Transpose(); Vec2 dp = posB - posA; Vec2 dA = RotAT * dp; Vec2 dB = RotBT * dp; Mat22 C = RotAT * RotB; Mat22 absC = Abs(C); Mat22 absCT = absC.Transpose();
body의 width는 body의 center를 기준으로 왼쪽 절반 길이와 오른쪽 절반 길이를 합친 것이다. 우리는 충돌 테스트하는데 있어서 그것의 절반이 필요하다. 그래서 hA와 hB에 절반의 width를 담는다.
position은 box의 center의 위치이다.
회전이 중요한데, 2차원에서 회전행렬은
[ cosθ -sinθ]
[ sinθ cosθ]
로 이루어져 있다. 여기에서 선형대수가 필요하다. 왜냐하면 이 회전행렬을 Transpose(전치행렬)시키기 때문이다.
하지만, 이 회전행렬에 대한 분석은 내가 이전의 포스트에서 했기 때문에, 나는 그것을 여기에다 복사 붙여넣기 할 것이다. 아래의 내용이 이해하기 어렵다면, 선형대수의 좌표변환 부분을 공부하는 것을 추천한다.
======================================================
{
상당히 여기 공부할 때 선형대수가 가장 많이 쓰인다. 좌표계 변환에 대해 나의 뇌를 적응하게 하기 위해서, 항상 반복 공부해야 한다. 그래서 여기에서 좀 제대로 써보자.
먼저 좌표계변환, 그러니까 기저 변환에 대해서 설명해보자.
뭔저 위의 예시에 맞게 World Coordinate Space와 Local Space라고 하고, 두 개를 W와 L로 칭하겠다.
우리는 W<->L 간에 변화를 쉽게 해야 한다.
선형대수에 따르면, W<->L로 한 벡터의 좌표계를 바꾸는 행렬에 대해서 말하자면
W<-L : 즉, Local Space에서 World Space로 바꿀 때, Local의 기저들을 World의 기저들의 관점에서 표현하는 행렬이다. 즉, Local의 기저들을 u0, u1, u2라고 하자.
위의 예제에 따르면 u0, u1, u2는 R^3에서 표현되어 있기 때문에, 이것은 Local Space의 기저들을 world space의 기저로 표현한 것과 다름 없다. 그래서
그러므로,
직관적으로 설명하자면 Local의 기저들을 World Space에서 표현했고, 그 local space의 vector를 P(W<-L)에 곱하게 된다면, world space에서의 좌표가 나오게 된다.
좀 더 직관적인 설명은 v_local 이 (1, 0, 0)이라 하면은 그 v'_world = (u0.x, u0.y, u0.z)가 되어서, Local의 x축을 world space로 표현하게 된다.
그렇다면 그 반대인 World Space -> Local Space는 무엇인가?
우리가 잘 알듯이, P(W<-L)의 역행렬을 취하면 되고, 거기에다가 그 행렬이 orthogonality의 특성이 있기 때문에 transpose(전치행렬)을 취하면 된다. 따라서
그리고 더 정확하게 말하자면 World Space의 기저들을 Local Space의 관점에서 표현한 것이다.
그러므로
이게 되고,
이렇게 된다. 따라서, 이제 우리는 위의 코드를 이해할 준비가 되었다.
(여기에서 기저변환 행렬의 elements들을 보면 왜 이렇게 해야 되는지 무언가 직관적인 이해가 오는 것 같다. P(W<-L) 같은 경우는 Local Space기저를 World로 표현하니, Local 벡터를 거기에 곱한다면 바로 World Vector가 나온다는게 직관적으로 이해된다. P(L<-W)가 좀 잘 이해가 안됐는데, World Space의 기저들을 Local Space로 표현한다는게, Local Basis들의 X component가 (1,0,0)을 Local X-axis를 표현하게 된다. 여튼 이런식으로 직관적 이해를하는 것은 기억력에 많은 도움이 될 것이다.)
이전의 AABB 처럼 clmaping 방식대로 한 축에 대해서 진행된다는 것에 주의해라.
하지만 이번엔 OBB의 local space 대로 한다.
우선 벡터 d = p - b.c를 통해 b의 center point에서 p로 가는 벡터를 구해놓는다.
이제 이것을 clamping 할껀데, OBB의 local space에서 진행한다.
q = b.c로 나중의 world space로 바꿀 때 편하게 하기 위해 이렇게 해놓는다.
그리고 반복문에서 첫 명령어로
float dist = Dot(d, b.u[i])가 되는데,
d라는 world space벡터를 b의 local space로 바꿔야 한다.
이렇게 되고, dot(d, b.u[i])를 통해 local space의 x축에 대한 dist를 알게 된다.
그래서 이 dist가 b.e[i]보다 크다면 b.e[i]로 clamp하고
dist가 -b.e[i]보다 작으면 dist = -b.e[i]로 clamp해서 사영시킨다.
그리고 나서 우리는 다시 그 거리를 world coordinate로 바꾸어야 한다.
즉,
여기에서 상당히 헷갈릴 수 있는데,
우리는 위의 행렬을 가지고 있고, v_local에서 x component만 가지고 있다는 것을 다시 기억해라.
그렇다면 P(W<-L) * (x, 0, 0)과 같게 되고, 이것은 dist * b.u[i] 와 똑같이 된다.
이제야 q += dist * b.u[i]가 이해 될 것이다.
우리는 이제 이것을 행렬로 표현하는 방법을 알게 될 것이다.
즉, OBB space에서 사영된 벡터 k를 구하고
P(W<-L) * k를 한 것을 q에 더해줘도 같은 결과이다.
}
======================================================
이제 위의 내용을 안다고 가정하고 회전행렬부터 다시 설명한다.
RotA는 한 벡터를 Local에서 bodyA의 world space로 바꾼다.
RotB는 한 벡터를 Local에서 bodyB의 world space로 바꾼다.
한 마디로 해당 body처럼 회전시킨다는 말이다.
RotAT는 한 벡터를 world에서 LocalA의 space로 바꾼다.
RotBT는 한 벡터를 world에서 LocalB의 space로 바꾼다.
한 마디로 해당 body의 원래 회전 안한 상태로 돌려버린다는 것이다.
dp 벡터는 bodyA의 중심에서 bodyB로가는 벡터이다. 이것은 나중의 테스트에서 중요한 역할을 한다.
dA는 dp벡터를 RotAT와 곱하는데, 이것은 그것을 LocalA space로 바꾼다.
dB는 dp벡터를 RotBT와 곱하는데, 이것은 그것을 LocalB space로 바꾼다.
이 또한 나중의 테스트를 위해 사용된다.
C = RotAT * RotB이다. 선형 대수의 특성상 행렬이 이럽게 곱하면 오른쪽에서 왼쪽순으로 누적이 된다. 그러니까 해당 회전행렬이 순서대로 적용된다는 것이다. 그렇다면 C 행렬은
Local -> World B -> LocalA의 순서대로 회전을 시키게 된다.
absC 행렬은 그 C행렬의 모든 component를 양수로 만들어버린다. 이것도 또한 나중의 테스트를 위해 필요하다.
absCT 행렬은 absC행렬을 전치행렬시킨다. (A * B)^T = B^T * A^T이기 때문에
absC행렬은 Local -> WorldA -> LocalB의 순서로 회전을 시킨다. 게다가 모든 컴포넌트가 양수이다.
이제 이것을 기억하고 SAT라는 충돌하는지를 보는 것을 한다.
2. Separating Axis Theorem(SAT) for Collision
// Box A faces Vec2 faceA = Abs(dA) - hA - absC * hB; if (faceA.x > 0.0f || faceA.y > 0.0f) return 0; // Box B faces Vec2 faceB = Abs(dB) - absCT * hA - hB; if (faceB.x > 0.0f || faceB.y > 0.0f) return 0;
상당히 간단해 보이지만, 이해하는데 상당히 많은 지식들을 요구한다.
우선 SAT가 무엇인지 이해할 필요가 있는데,
간단하게 말하자면, boxA의 중심에서 boxB로 가는 중심을 어떤 축에 대해 사영시킨다. 그리고 각 box의 half extent를 또한 그 축에 사영시킨다. 그 한 축의 길이를 t라고 하고, 사영된 각 box의 half extent의 합을 k라 했을 때, t - k < 0이면 충돌하고 있다는 것이고. t + k > 0이면 두 box가 충돌하고 있지 않다. SAT의 정의에 따라, t + k > 0인 것이 하나라도 있으면 두 오브젝트는 충돌하고 있지 않는다고 한다. 그래서 우리는 이것을 할 것이다.
하지만 우리가 하는 것은 현재 2D이기 때문에 가장 쉬운 방법은 사영시킬 벡터들을 local A의 space로 바꾸어서 x축과 y축에 대해서 SAT 검사를 하고, local B의 space로 바꾸어서 x축과 y축에 대해서 SAT 검사를 하면 빠르게 처리할 수 있다.
dA는 dp(그림에서 빨간색 벡터)를 RotAT(world에서 LocalA space)를 곱해 변환한 것이다.
그래서 빨간색 벡터를 LocalA space로 바꾸어서 x축과 y축에 대해 비교할 수 있다. hA는 이미 LocalA space에 있기 때문에 그대로 빼면된다. hB는 local B의 space에 있기 때문에, absC(Local -> World B -> LocalA)로 다시 local A로 바꾸어준다. 그래서 그것을 빼주면 , 위의 t - k를 한 것이된다. 그래서 한 축이라도 0보다 큰 게 있다면 충돌하지 않고 있는 것이다. 여기에서 Abs를 해서 그 dA 컴포넌트를 양수로 바꾸어주었는데, 이것을 한 이유는 -가 있으면 SAT 테스트가 여기저기 부호를 검사하면서 하기 때문에 단순히 길이로만 측정한다고 생각하면 Abs를해서 쉽게 길이만 비교할 수 있다. 그래서 hA 또한 양수이고, hB를 회전할 때에도, absC를 통해 회전시키기 때문에, 회전이 되더라도, 길이로만 측정할 수 있게 해준다.
A의 관점으로도 해주었으니, B의 관점으로도 해준다. 이로서 빠른 충돌 테스트가 끝났고, 이제 이 충돌처리를 해야할 시간이다. 여기에서 우리는 box의 축을 사용할지를 결정해야 된다. ~~~ 나중에 계속.
=====================================================
내 엔진에 연습할려고 직접 typing해서 box2d lite를 구현했고.
이제 그것을 하나씩 이해하면서 주석을 달았다. 아직 이 포스트가 완성되기 전에 이것을 참고해도 좋다.
// Box vertex and edge numbering: // // ^ y // | // e1 // v2 ------ v1 // | | // e2 | | e4 --> x // | | // v3 ------ v4 // e3 enum Axis { FACE_A_X, FACE_A_Y, FACE_B_X, FACE_B_Y }; enum EdgeNumbers { NO_EDGE = 0, EDGE1, EDGE2, EDGE3, EDGE4 }; struct ClipVertex { ClipVertex() { fp.value = 0; } Chan::ChVector2 v; Chan::FeaturePair fp; }; inline void Flip(Chan::FeaturePair& fp) { Chan::Swap(fp.e.inEdge1, fp.e.inEdge2); Chan::Swap(fp.e.outEdge1, fp.e.outEdge2); } int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], const Chan::ChVector2& normal, Chan::ChReal offset, char clipEdge) { // Start with no output pints int numOut = 0; // Calculate the distance of end points to the line // It's equivalent to the distance from vertex to plane in 3D. // If distance0 > 0 , the vertex in on the side of plane normal. Chan::ChReal distance0 = dot(normal, vIn[0].v) - offset; Chan::ChReal distance1 = dot(normal, vIn[1].v) - offset; // If the points are behind the plane, meaning on the side oppostite the normal, // So, we have to keep it. we don't need to clip it. if (distance0 <= Chan::ChReal(0.0)) vOut[numOut++] = vIn[0]; if (distance1 <= Chan::ChReal(0.0)) vOut[numOut++] = vIn[1]; // If the points are on different sides of the plane // We have to clip it. if (distance0 * distance1 < Chan::ChReal(0.0)) { // Find intersection point of edge and plane // You will get to know why this line should be like this // If you draw graph on your paper. Chan::ChReal interp = distance0 / (distance0 - distance1); vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); // If vIn[0] is on the side of normal // That is, you should put the clipped information on the Out ClipVertex if (distance0 > Chan::ChReal(0.0)) { vOut[numOut].fp = vIn[0].fp; vOut[numOut].fp.e.inEdge1 = clipEdge; // Reference Face's clipEdge is the inEdge of this clipped vertex vOut[numOut].fp.e.inEdge2 = NO_EDGE; // Delete previous inEdge // So now this clip vertex consists of inEdge1(from reference facae) and outEdge2(from incident face) } // in this case, vIn[0] is on the opposite side of normal. // So vIn[1] is on the side of normal. you should put the clipped information on the Out ClipVertex else { vOut[numOut].fp = vIn[1].fp; vOut[numOut].fp.e.outEdge1 = clipEdge; // Reference Face's clipedge is outedge of this cliped vertex vOut[numOut].fp.e.outEdge2 = NO_EDGE; // Delete previous outEdge // So now this clip vertex consists of inEdge2(from incident face) and outEdge1(from reference face) } ++numOut; } return numOut; } void ComputeIncidentEdge(ClipVertex c[2], const Chan::ChVector2& h, // half extents const Chan::ChVector2& pos, const Chan::ChMat22& Rot, const Chan::ChVector2& normal) { // The normal is from the reference box. Convert it. // to the incident boxe's frame and flip sign. Chan::ChMat22 RotT = Rot.Transpose(); // World to Local Chan::ChVector2 n = -(RotT * normal); // From World Normal to Local Chan::ChVector2 nAbs = Abs(n); // Box vertex and edge numbering: // ^ y // | // e1 // v2 ------ v1 // | | // e2 | | e4 --> x // | | // v3 ------ v4 // e3 // nAbs.x > nAbs.y means that we have to check left or right edges on the box // the incident edges of incident face will be e1, e4, e3 with positive n.x // with negative n.x, the edges will be e1, e2, e3. // else if(nAbs.x < nAbs.y) means that we have to check up or down edges on the box // the incident edges of incident face will be e2, e1, e4 with positinve n.y. // with negative n.y, the edges will be e2, e3, e4. // // You will get to know why we use Absolute Vector to check which axis to use between x-axis and y-axis // if you draw and write your own box on the paper. // The notion of in/out Edge is related to the winding of the edges. // The box above is specified in Counter-Clock Wise(CCW) winding. // So, inEdge means the first edge to one vertex, and outEdge means the second edge out of one vertex. // And in/outEdge1 means edges of reference face. // in/outEdge2 means edges of Incident face. if (nAbs.x > nAbs.y) // X-axis is incident area { // Right X-axis if (Chan::Sign(n.x) > Chan::ChReal(0.0)) { c[0].v.Set(h.x, -h.y); // Vertex 4 c[0].fp.e.inEdge2 = EDGE3; c[0].fp.e.outEdge2 = EDGE4; c[1].v.Set(h.x, h.y); // Vertex 1 c[1].fp.e.inEdge2 = EDGE4; c[1].fp.e.outEdge2 = EDGE1; } else // Left X-axis { c[0].v.Set(-h.x, h.y); // Vertex 2 c[0].fp.e.inEdge2 = EDGE1; c[0].fp.e.outEdge2 = EDGE2; c[1].v.Set(-h.x, -h.y); // Vertex 3 c[1].fp.e.inEdge2 = EDGE2; c[1].fp.e.outEdge2 = EDGE3; } } else // Y-axis is incident area { // Upper Y-axis if (Chan::Sign(n.y) > Chan::ChReal(0.0)) { c[0].v.Set(h.x, h.y); // Vertex 1 c[0].fp.e.inEdge2 = EDGE4; c[0].fp.e.outEdge2 = EDGE1; c[1].v.Set(-h.x, h.y); // Vertex 2 c[1].fp.e.inEdge2 = EDGE1; c[1].fp.e.outEdge2 = EDGE2; } else // Lower Y-axis { c[0].v.Set(-h.x, -h.y); // Vertex 3 c[0].fp.e.inEdge2 = EDGE2; c[0].fp.e.outEdge2 = EDGE3; c[1].v.Set(h.x, -h.y); // Vertex 4 c[1].fp.e.inEdge2 = EDGE3; c[1].fp.e.outEdge2 = EDGE4; } } // Rotate and Translate the clip vertex to the original vertex position. (from local to world) c[0].v = pos + Rot * c[0].v; c[1].v = pos + Rot * c[1].v; } // The normal points from A to B int Chan::Collide(Contact* contacts, chLiteBody* bodyA, chLiteBody* bodyB) { // Setup Chan::ChVector2 hA = Chan::ChReal(0.5) * bodyA->width; Chan::ChVector2 hB = Chan::ChReal(0.5) * bodyB->width; ChVector2 posA = bodyA->position; ChVector2 posB = bodyB->position; // Local to World Rotation Transform ChMat22 RotA(bodyA->rotation), RotB(bodyB->rotation); // World to Local Transform ChMat22 RotAT = RotA.Transpose(); ChMat22 RotBT = RotB.Transpose(); ChVector2 dp = posB - posA; ChVector2 dA = RotAT * dp; ChVector2 dB = RotBT * dp; ChMat22 C = RotAT * RotB; // RotAT(World to Local A) * RotB (Local to World B) ChMat22 absC = Abs(C); // Transpose(absC) = RotB^t * RotAT^t // = RotBT(World to LocalB) * RotA (Local to World) ChMat22 absCT = absC.Transpose(); // Box A faces (SAT) ChVector2 faceA = Abs(dA) - hA - absC * hB; if (faceA.x > ChReal(0.0) || faceA.y > ChReal(0.0)) return 0; // Box B faces (SAT) ChVector2 faceB = Abs(dB) - absCT * hA - hB; if (faceB.x > ChReal(0.0) || faceB.y > ChReal(0.0)) return 0; // Find best axis Axis axis; ChReal t_separation; ChVector2 normal; // Box A faces axis = FACE_A_X; t_separation = faceA.x; /* * The way of determining the normal is that, if you assume the FACE_A_X should be a least penetration axis * dA is A's Local Space from A center to B center, and if its x component is over 0, * The colliding direction of A with B is Right X axis. * And each column of rotatio matrix, specifically the local to world matrix, means * [ Vec1(Local Axis X in terms of World) Vec2(Local Axis Y in terms of World) ]. * So you can use just col1 or -col1 if the normal is pointing toward left. * * To simplify the explanation according to the Erin Catto's presentation, * In 2D, the separating axis is a face normal. */ normal = dA.x > ChReal(0.0) ? RotA.col1 : -RotA.col1; const ChReal relativeTol = ChReal(0.95); const ChReal absoluteTol = ChReal(0.01); // To find the least penetration axis in the consistent way, // favor original axis to get better float-point comparison // The reason why we have to find the least penetration axis, you will get to know // if you imagine the sitatuon, where two boxes overlaps and you have to calculate each penetration axis value manually. // Erin's explanation on this : https://pybullet.org/Bullet/phpBB3/viewtopic.php?f=4&t=398 if (faceA.y > relativeTol * t_separation + absoluteTol * hA.y) { axis = FACE_A_Y; t_separation = faceA.y; normal = dA.y > ChReal(0.0) ? RotA.col2 : -RotA.col2; } // Box B faces if (faceB.x > relativeTol * t_separation + absoluteTol * hB.x) { axis = FACE_B_X; t_separation = faceB.x; normal = dB.x > ChReal(0.0) ? RotB.col1 : -RotB.col1; } if (faceB.y > relativeTol * t_separation + absoluteTol * hB.y) { axis = FACE_B_Y; t_separation = faceB.y; normal = dB.y > ChReal(0.0) ? RotB.col2 : -RotB.col2; } /* * Clipping Setup : * 1. Identify Reference Face -> We already know what is reference face with least seperating axis * 2. Identify Incident Face -> We will calculate the incident face with ComputeIncidentEdge() function * 3. And then clip the face to the edge, using Sutherland-Hodgman clipping, using ClipSegmentToLine() function. * refer necessarily to the Erin Catto's presentation for Box-Box Clipping * You Should remember this diagram again // Box vertex and edge numbering: // // ^ y // | // e1 // v2 ------ v1 // | | // e2 | | e4 --> x // | | // v3 ------ v4 // e3 * */ // Setup clipping plane data based on the separating axis ChVector2 frontNormal, sideNormal; ClipVertex incidentEdge[2]; ChReal front, negSide, posSide; char negEdge, posEdge; // compute the clipping lines and the line segment to be clipped. switch (axis) { case FACE_A_X: { frontNormal = normal; front = dot(posA, frontNormal) + hA.x; /* * ClipVertex has the next structure * struct ClipVertex { ClipVertex() { fp.value = 0; } Chan::ChVector2 v; Chan::FeaturePair fp; }; * FeaturePair has the next union structure * union FeaturePair { struct Edges { char inEdge1; char outEdge1; char inEdge2; char outEdge2; } e; int value; }; * * FeaturePair will be used to specify which edges are related to the collision area. * And the value will be used to identify the old contact to accumulate the impulses. * So ComputeIncidentEdge will not use "int value" member. * And, ComputeIncidentEdge will fill inEdge2, outEdge2. * I guess so far the 2 means the second box(B). So, We will calculate B's incident edge. * In case of the least penetration axis from Body B, A will be assumed as Body B, opposite one. * It will be easire for you to think of 2 as a opposite one. * And 1 means the standard one. */ // Informations needed to get clip the incident edge in ClipSegmentToLine function sideNormal = RotA.col2; ChReal side = dot(posA, sideNormal); negSide = -side + hA.y; posSide = side + hA.y; negEdge = EDGE3; posEdge = EDGE1; // Informations needed to get clip the incident edge in ClipSegmentToLine function /* * ClipSegmentToLine uses Sutherland-Hodgman clipping. * It means extending the edges to the plane and clipping incident edges * so, the informations above for the ClipSegmentToLine function are related to edges and planes * especially, negSide and posSide means offset of plane. However, in 2D, There is no plane. * So, it is the line offset. But, the reason why negside = -side + hA.y is that, * the negSide has the opposite normal. So, If it has the same normal with posSide, * negSide offset will be side - hA.y. Since it has opposite direction, we have to negate the negSide. * ClipSegmentToLine will use these offset to distinguish whether the clip vertex is on the plane(line) normal or not. * If clip vertex is on the plane(line) normal, we have to clip it. Otherwise, we have to keep it, because it's begine the * plane. */ ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal); } break; case FACE_A_Y: { frontNormal = normal; front = dot(posA, frontNormal) + hA.y; sideNormal = RotA.col1; ChReal side = dot(posA, sideNormal); negSide = -side + hA.x; posSide = side + hA.x; negEdge = EDGE2; posEdge = EDGE4; ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal); } break; case FACE_B_X: { frontNormal = -normal; front = dot(posB, frontNormal) + hB.x; sideNormal = RotB.col2; ChReal side = dot(posB, sideNormal); negSide = -side + hB.y; posSide = side + hB.y; negEdge = EDGE3; posEdge = EDGE1; ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal); } break; case FACE_B_Y: { frontNormal = -normal; front = dot(posB, frontNormal) + hB.y; sideNormal = RotB.col1; ChReal side = dot(posB, sideNormal); negSide = -side + hB.x; posSide = side + hB.x; negEdge = EDGE2; posEdge = EDGE4; ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal); } break; } /* * Now you get the incident edge from the ComputeIncidentEdge() * And you also hold the information of side edges of the reference edge * The reason why we get the side information is for cliipping the incident edge with side edge. * It's Sutherland-Hodgman algorithm for clipping polygons. */ // clip other face with 5 box planes (1 face plane, 4 edge plane) ClipVertex clipPoints1[2]; ClipVertex clipPoints2[2]; int np; // Clip to box side 1 np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge); if (np < 2) return 0; // Clip to negative box side 1 np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge); if (np < 2) return 0; // Now clipoints2 contains the clipping points. // Due to roundoff, it is possible that clipping removes all points. int numContacts = 0; for (int i = 0; i < 2; ++i) { ChReal separation = dot(frontNormal, clipPoints2[i].v) - front; // Check if the clipped veritces are behind the reference face // If it is not, the vertex is not the colliding point if (separation <= 0) { contacts[numContacts].separation = separation; contacts[numContacts].normal = normal; // slide contact point onto reference face (easy to cull) // imagine the contact manifold. if the contact manifold will not be modifed to the reference face, // the contact resolution will work in the wrong way a little. // https://www.reddit.com/r/box2d/comments/aim8rm/box2d_lite_collision/ // I did self-question and self-answer on this reddit post. contacts[numContacts].position = clipPoints2[i].v - separation * frontNormal; contacts[numContacts].feature = clipPoints2[i].fp; // this collision assumes that reference face is boxA. // Because the incident edge is always put on in/outEdge2, boxB. // It means the edges will be calculated in terms of A. // So If the axis will be from B, we need to flip the feature edge specifying where the edge is from. if (axis == FACE_B_X || axis == FACE_B_Y) Flip(contacts[numContacts].feature); ++numContacts; } } return numContacts; }
댓글 없음:
댓글 쓰기