1861 lines
52 KiB
C++
1861 lines
52 KiB
C++
#ifndef __COLLISION_CHECKER__
|
|
#define __COLLISION_CHECKER__
|
|
|
|
#include "K3DTypes.h"
|
|
#include <assert.h>
|
|
|
|
class PrimitiveCollisionCheckPackage
|
|
{
|
|
private:
|
|
static inline float abs( float a )
|
|
{
|
|
return a > 0 ? a : -a;
|
|
}
|
|
|
|
static inline float vec_distance( const K3DVector &u )
|
|
{
|
|
return K3DVectorLength(u);
|
|
}
|
|
static inline float vec_distance( const K3DVector &u, const K3DVector &v )
|
|
{
|
|
K3DVector vec(u.x - v.x, u.y - v.y, u.z - v.z);
|
|
return K3DVectorLength(vec);
|
|
}
|
|
static inline void vec_sub_vec( K3DVector &r, const K3DVector &p, const K3DVector &q )
|
|
{
|
|
r = p - q;
|
|
}
|
|
static inline float vec_dot_vec( const K3DVector& p, const K3DVector& q )
|
|
{
|
|
return K3DVectorDot(p, q);
|
|
}
|
|
static inline void vec_cross_vec( K3DVector& r, const K3DVector& p, const K3DVector& q )
|
|
{
|
|
r.x = p.y*q.z - p.z*q.y;
|
|
r.y = p.z*q.x - p.x*q.z;
|
|
r.z = p.x*q.y - p.y*q.x;
|
|
//K3DVectorCross(r,p,q);
|
|
}
|
|
static inline void vec_normalize( K3DVector& r )
|
|
{
|
|
Normalize(r);
|
|
}
|
|
|
|
static inline void make_vector(K3DVector &res, const K3DVector &p, const K3DVector&q)
|
|
{
|
|
res = q - p;
|
|
}
|
|
|
|
/// { [sonador][7.1.7]충돌 처리 개선(Swept Shpere Collision Detection)
|
|
static inline float vec_distance_from_plane( const K3DVector& plane_normal, const K3DVector& point_lies_on_plane, const K3DVector& p )
|
|
{
|
|
return vec_dot_vec( plane_normal, p ) - ( vec_dot_vec( plane_normal, point_lies_on_plane ) );
|
|
}
|
|
|
|
static inline bool check_point_in_triangle( const K3DVector& p, const K3DVector& p1, const K3DVector& p2, const K3DVector& p3 )
|
|
{
|
|
K3DVector edge21 = p2 - p1;
|
|
K3DVector edge31 = p3 - p1;
|
|
|
|
float a = vec_dot_vec( edge21, edge21 );
|
|
float b = vec_dot_vec( edge21, edge31 );
|
|
float c = vec_dot_vec( edge31, edge31 );
|
|
float ac_bb = ( a * c ) - ( b * b );
|
|
|
|
K3DVector vp( p.x - p1.x, p.y - p1.y, p.z - p1.z );
|
|
|
|
float d = vec_dot_vec( vp, edge21 );
|
|
float e = vec_dot_vec( vp, edge31 );
|
|
float x = ( d * c ) - ( e * b );
|
|
float y = ( e * a ) - ( d * b );
|
|
float z = x + y - ac_bb;
|
|
|
|
return (( ( ( (unsigned int&)z ) & ~( ( (unsigned int&)x ) | ( (unsigned int&)y ) ) ) & 0x80000000 )) != 0; // C4800 워닝 처리
|
|
}
|
|
|
|
static inline bool get_lowest_root( float a, float b, float c, float max_root, float& root )
|
|
{
|
|
float determinant = b * b - 4.0f * a * c;
|
|
|
|
if( determinant < 0.0f ) return false;
|
|
|
|
float sqrt_d = ::sqrt( determinant );
|
|
float r1 = ( -b - sqrt_d ) / ( 2.0f * a );
|
|
float r2 = ( -b + sqrt_d ) / ( 2.0f * a );
|
|
|
|
if( r1 > r2 )
|
|
{
|
|
float temp = r2;
|
|
r2 = r1;
|
|
r1 = temp;
|
|
}
|
|
|
|
if( r1 > 0 && r1 < max_root )
|
|
{
|
|
root = r1;
|
|
return true;
|
|
}
|
|
|
|
if( r2 > 0 && r2 < max_root )
|
|
{
|
|
root = r2;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
// }
|
|
|
|
public:
|
|
|
|
/** cube 일때 K3DVector 구성 : 별 상관은 없지만서도.....
|
|
(front, top, left ), (front, top, right), (front, bottom, right), (front, bottom, left ) - z, y, x
|
|
(back , top, right), (back , top, left ), (back , bottom, left ), (back , bottom, right) - z, y, x */
|
|
static inline const K3DVector get_cube_vertex(const int &n,
|
|
const float &minx, const float &maxx,
|
|
const float &miny, const float &maxy,
|
|
const float &minz, const float &maxz )
|
|
{
|
|
K3DVector vtx;
|
|
|
|
switch(n) {
|
|
case 0:
|
|
vtx.x = minx; vtx.y = miny; vtx.z = minz;
|
|
break;
|
|
case 1:
|
|
vtx.x = maxx; vtx.y = miny; vtx.z = minz;
|
|
break;
|
|
case 2:
|
|
vtx.x = maxx; vtx.y = maxy; vtx.z = minz;
|
|
break;
|
|
case 3:
|
|
vtx.x = minx; vtx.y = maxy; vtx.z = minz;
|
|
break;
|
|
case 4:
|
|
vtx.x = maxx; vtx.y = miny; vtx.z = maxz;
|
|
break;
|
|
case 5:
|
|
vtx.x = minx; vtx.y = miny; vtx.z = maxz;
|
|
break;
|
|
case 6:
|
|
vtx.x = minx; vtx.y = maxy; vtx.z = maxz;
|
|
break;
|
|
case 7:
|
|
vtx.x = maxx; vtx.y = maxy; vtx.z = maxz;
|
|
break;
|
|
}
|
|
|
|
return vtx;
|
|
}
|
|
static inline void add_cube( K3DVector* rescube, const K3DVector* cube1, const K3DVector *cube2 )
|
|
{
|
|
if(cube1 == NULL || cube2 == NULL) return;
|
|
|
|
int i;
|
|
K3DVector minvtx = cube1[0], maxvtx = cube1[0];
|
|
for(i = 1; i < 8; ++i) {
|
|
if(cube1[i].x < minvtx.x) minvtx.x = cube1[i].x;
|
|
if(cube1[i].x > maxvtx.x) maxvtx.x = cube1[i].x;
|
|
if(cube1[i].y < minvtx.y) minvtx.y = cube1[i].y;
|
|
if(cube1[i].y > maxvtx.y) maxvtx.y = cube1[i].y;
|
|
if(cube1[i].z < minvtx.z) minvtx.z = cube1[i].z;
|
|
if(cube1[i].z > maxvtx.z) maxvtx.z = cube1[i].z;
|
|
}
|
|
for(i = 0; i < 8; ++i) {
|
|
if(cube2[i].x < minvtx.x) minvtx.x = cube2[i].x;
|
|
if(cube2[i].x > maxvtx.x) maxvtx.x = cube2[i].x;
|
|
if(cube2[i].y < minvtx.y) minvtx.y = cube2[i].y;
|
|
if(cube2[i].y > maxvtx.y) maxvtx.y = cube2[i].y;
|
|
if(cube2[i].z < minvtx.z) minvtx.z = cube2[i].z;
|
|
if(cube2[i].z > maxvtx.z) maxvtx.z = cube2[i].z;
|
|
}
|
|
|
|
for(i = 0; i < 8; ++i)
|
|
rescube[i] = get_cube_vertex(i, minvtx.x, maxvtx.x, minvtx.y, maxvtx.y, minvtx.z, maxvtx.z);
|
|
}
|
|
|
|
static inline float triangle_collide_edge_twosides( const K3DVector& p1, const K3DVector& p2, const K3DVector& p3,
|
|
const K3DVector& e1, const K3DVector& e2, K3DVector& res )
|
|
{
|
|
return imp_triangle_collide_edge_twosides( p1,p2,p3, e1,e2, res );
|
|
}
|
|
|
|
static inline bool triangle_collide_edge( const K3DVector& p1, const K3DVector& p2, const K3DVector& p3,
|
|
const K3DVector& e1, const K3DVector& e2 )
|
|
{
|
|
return imp_triangle_collide_edge( p1,p2,p3, e1,e2 );
|
|
}
|
|
|
|
// p1 +-------+ p2
|
|
// | |
|
|
// | |
|
|
// p4 +-------+ p3
|
|
static inline bool quad_collide_edge( const K3DVector& p1, const K3DVector& p2, const K3DVector& p3, const K3DVector& p4,
|
|
const K3DVector& e1, const K3DVector& e2 )
|
|
{
|
|
return imp_quad_collide_edge( p1,p2,p3,p4, e1,e2 );
|
|
}
|
|
|
|
static inline bool triangle_collide_triangle( const K3DVector &a1, const K3DVector &a2, const K3DVector &a3,
|
|
const K3DVector &b1, const K3DVector &b2, const K3DVector &b3 )
|
|
{
|
|
return imp_triangle_collide_triangle( a1,a2,a3, b1,b2,b3 );
|
|
}
|
|
|
|
/// buggy~
|
|
static inline bool triangle_collide_sphere( const K3DVector &p1, const K3DVector &p2, const K3DVector &p3,
|
|
const K3DVector &s, float r )
|
|
{
|
|
return imp_triangle_collide_sphere( p1,p2,p3, s,r );
|
|
}
|
|
|
|
/// { [sonador][7.1.7]충돌 처리 개선(Swept Shpere Collision Detection)
|
|
static inline bool triangle_collide_swept_unit_sphere(
|
|
const K3DVector &p1
|
|
, const K3DVector &p2
|
|
, const K3DVector &p3
|
|
, const K3DVector &base
|
|
, const K3DVector &velocity
|
|
, K3DVector& collision
|
|
, float& t )
|
|
{
|
|
return imp_triangle_collide_swept_unit_sphere( p1, p2, p3, base, velocity, collision, t );
|
|
}
|
|
// }
|
|
|
|
// rect means z-aligned rectangle(or cube) z member of K3DVector must be 0
|
|
// p1 +-------+ p2
|
|
// | |
|
|
// | |
|
|
// p4 +-------+ p3
|
|
static inline bool rect_collide_rect( const K3DVector *pa, const K3DVector *pb )
|
|
{
|
|
return imp_rect_collide_rect( pa, pb );
|
|
}
|
|
|
|
static inline bool point_collide_cube( const K3DVector& p,
|
|
float minx, float maxx,
|
|
float miny, float maxy,
|
|
float minz, float maxz )
|
|
{
|
|
return imp_point_collide_cube( p, minx, maxx, miny, maxy, minz, maxz );
|
|
}
|
|
|
|
static inline bool edge_collide_cube( const K3DVector& e1, const K3DVector& e2,
|
|
float minx, float maxx,
|
|
float miny, float maxy,
|
|
float minz, float maxz )
|
|
{
|
|
return imp_edge_collide_cube( e1, e2, minx, maxx, miny, maxy, minz, maxz );
|
|
}
|
|
|
|
static inline bool triangle_collide_cube( const K3DVector& p1, const K3DVector& p2, const K3DVector& p3,
|
|
float minx, float maxx,
|
|
float miny, float maxy,
|
|
float minz, float maxz )
|
|
{
|
|
return imp_triangle_collide_cube( p1,p2,p3, minx,maxx,miny,maxy,minz,maxz );
|
|
}
|
|
|
|
//
|
|
// cube의 K3DVector 구성 : 정면(front)을 기준으로, 방향(CW or CCW)은 따지지 않는다.
|
|
// (front, top, left ), (front, top, right), (front, bottom, right), (front, bottom, left ) - z, y, x
|
|
// (back , top, right), (back , top, left ), (back , bottom, left ), (back , bottom, right) - z, y, x
|
|
//
|
|
//
|
|
// *-----------* *-----------*
|
|
// / c6 c5 /| / c2 c1 /|
|
|
// / / | / / | p1 +-------+ p2
|
|
// *-----------* | *-----------* | | |
|
|
// | c1 c2 | + | c5 c6 | + | |
|
|
// | | / | | / p4 +-------+ p3
|
|
// | c4 c3 |/ | c8 c7 |/
|
|
// *-----------* *-----------*
|
|
//
|
|
// front back quad
|
|
//
|
|
|
|
static inline bool edge_collide_rotcube( const K3DVector* edge, const K3DVector *cube )
|
|
{
|
|
return imp_edge_collide_nonuniformcube( edge, cube );
|
|
}
|
|
|
|
static inline bool triangle_collide_rotcube( const K3DVector* triangle, const K3DVector* cube )
|
|
{
|
|
return imp_triangle_collide_rotcube( triangle, cube );
|
|
}
|
|
|
|
static inline bool rotcube_collide_rotcube( const K3DVector* cube1, const K3DVector* cube2 )
|
|
{
|
|
return imp_rotcube_collide_rotcube( cube1, cube2 );
|
|
}
|
|
|
|
static inline bool point_collide_nonuniformcube( const K3DVector* point, const K3DVector* cube )
|
|
{
|
|
return imp_point_collide_nonuniformcube( point, cube );
|
|
}
|
|
|
|
static inline bool edge_collide_nonuniformcube( const K3DVector* edge, const K3DVector *cube )
|
|
{
|
|
return imp_edge_collide_nonuniformcube( edge, cube );
|
|
}
|
|
|
|
static inline bool triangle_collide_nonuniformcube( const K3DVector* triangle, const K3DVector* cube )
|
|
{
|
|
return imp_triangle_collide_nonuniformcube( triangle, cube );
|
|
}
|
|
|
|
static inline bool quad_collide_nonuniformcube( const K3DVector* quad, const K3DVector* cube )
|
|
{
|
|
return imp_quad_collide_nonuniformcube( quad, cube );
|
|
}
|
|
|
|
static inline bool nonuniformcube_collide_nonuniformcube( const K3DVector* cube1, const K3DVector *cube2 )
|
|
{
|
|
return imp_nonuniformcube_collide_nonuniformcube( cube1, cube2 );
|
|
}
|
|
|
|
static inline bool point_collide_cylinder( const K3DVector* point, const K3DVector* APole, const K3DVector* BPole, float radius )
|
|
{
|
|
return imp_point_collide_cylinder( point, APole, BPole, radius );
|
|
}
|
|
|
|
private:
|
|
|
|
/// 충돌이 일어나면 0 <= t < 1.0f 인 t 값을, 그렇지 않으면 1.0f를 리턴 (pVel = pEnd - pStart)
|
|
static inline float imp_plane_collide_edge( const K3DPlane *pPlane, const K3DVector *pStart, const K3DVector *pVel)
|
|
{
|
|
//float dot1 = plane.n.Dot(start) + plane.d;
|
|
float dot1 = pPlane->a * pStart->x + pPlane->b * pStart->y + pPlane->c * pStart->z + pPlane->d;
|
|
//float dot2 = plane.n.Dot(vel);
|
|
float dot2 = pPlane->a * pVel->x + pPlane->b * pVel->y + pPlane->c * pVel->z;
|
|
|
|
if(fabsf(dot2) < D3DX_16F_EPSILON) return 1.0f;
|
|
|
|
float t = -1.0f * dot1 / dot2;
|
|
|
|
return t;
|
|
}
|
|
|
|
/// 충돌이 일어나면 0 <= t < 1.0f 인 t 값을, 그렇지 않으면 1.0f를 리턴
|
|
static inline float imp_triangle_collide_edge_twosides( const K3DVector& v1, const K3DVector& v2, const K3DVector& v3,
|
|
const K3DVector& start, const K3DVector& end, K3DVector& res )
|
|
{
|
|
//K3DVector vec1, vec2, norm;
|
|
|
|
//vec1.x = v2.x - v1.x;
|
|
//vec1.y = v2.y - v1.y;
|
|
//vec1.z = v2.z - v1.z;
|
|
//vec2.x = v3.x - v1.x;
|
|
//vec2.y = v3.y - v1.y;
|
|
//vec2.z = v3.z - v1.z;
|
|
|
|
//norm.Cross(vec1, vec2);
|
|
//norm = CrossProduct(vec1, vec2);
|
|
//norm.Normalize();
|
|
//Normalize(norm);
|
|
//float d = norm.Dot(v1) * -1.0f;
|
|
//float d = DotProduct(norm, v1) * -1.0f;
|
|
|
|
//plane_t plane;
|
|
//plane.n = norm;
|
|
//plane.d = d;
|
|
|
|
K3DPlane plane;
|
|
K3DPlaneFromPoints(plane, v1, v2, v3);
|
|
|
|
K3DVector vel = end - start;
|
|
//float t = RayPlane(start, vel, plane);
|
|
float t = imp_plane_collide_edge(&plane, &start, &vel);
|
|
|
|
if(t < 0 || t >= 1.0f) return 1.0f; // 충돌이 일어나지 않음
|
|
|
|
res = start + vel * t; // 접점
|
|
|
|
K3DPoint p1, p2, p3, pRes;
|
|
float nabsx = fabsf(plane.a);
|
|
float nabsy = fabsf(plane.b);
|
|
float nabsz = fabsf(plane.c);
|
|
|
|
if(nabsx >= nabsy && nabsx >= nabsz)
|
|
{
|
|
//for(int i = 0; i < 3; i++)
|
|
//{
|
|
// v[i].x = verts[i].y;
|
|
// v[i].y = verts[i].z;
|
|
//}
|
|
|
|
p1.x = v1.y;
|
|
p1.y = v1.z;
|
|
p2.x = v2.y;
|
|
p2.y = v2.z;
|
|
p3.x = v3.y;
|
|
p3.y = v3.z;
|
|
|
|
pRes.x = res.y;
|
|
pRes.y = res.z;
|
|
}
|
|
else if(nabsy >= nabsz && nabsy >= nabsx)
|
|
{
|
|
//for(int i = 0; i < 3; i++)
|
|
//{
|
|
// v[i].x = verts[i].x;
|
|
// v[i].y = verts[i].z;
|
|
//}
|
|
|
|
p1.x = v1.x;
|
|
p1.y = v1.z;
|
|
p2.x = v2.x;
|
|
p2.y = v2.z;
|
|
p3.x = v3.x;
|
|
p3.y = v3.z;
|
|
|
|
pRes.x = res.x;
|
|
pRes.y = res.z;
|
|
}
|
|
else if(nabsz >= nabsx && nabsz >= nabsy)
|
|
{
|
|
//for(int i = 0; i < 3; i++)
|
|
//{
|
|
// v[i].x = verts[i].x;
|
|
// v[i].y = verts[i].y;
|
|
//}
|
|
|
|
p1.x = v1.x;
|
|
p1.y = v1.y;
|
|
p2.x = v2.x;
|
|
p2.y = v2.y;
|
|
p3.x = v3.x;
|
|
p3.y = v3.y;
|
|
|
|
pRes.x = res.x;
|
|
pRes.y = res.y;
|
|
}
|
|
else // 도달해서는 안되는 코드
|
|
{
|
|
//assert(0);
|
|
return 1.0f;
|
|
}
|
|
|
|
K3DPoint side, nside, ref, vp;
|
|
//int a, b, c;
|
|
|
|
for(int i = 0; i < 3; i++)
|
|
{
|
|
//a = i;
|
|
//b = (i + 1) % 3;
|
|
//c = (i + 2) % 3;
|
|
|
|
//side = p[b] - p[a];
|
|
//nside.Set(side.y, -side.x);
|
|
|
|
//ref = p[c] - p[a];
|
|
//vp = pRes - p[a];
|
|
|
|
switch(i)
|
|
{
|
|
case 0:
|
|
side = p2 - p1;
|
|
ref = p3 - p1;
|
|
vp = pRes - p1;
|
|
break;
|
|
|
|
case 1:
|
|
side = p3 - p2;
|
|
ref = p1 - p2;
|
|
vp = pRes - p2;
|
|
break;
|
|
|
|
case 2:
|
|
side = p1 - p3;
|
|
ref = p2 - p3;
|
|
vp = pRes - p3;
|
|
break;
|
|
}
|
|
|
|
//nside.Set(side.y, -side.x);
|
|
//if(nside.Dot(ref) * nside.Dot(vp) < 0) return 1.0f; // 충돌이 일어나지 않음
|
|
|
|
nside.x = side.y;
|
|
nside.y = -side.x;
|
|
if((nside.x * ref.x + nside.y * ref.y) * (nside.x * vp.x + nside.y * vp.y) < 0) return 1.0f; // 충돌이 일어나지 않음
|
|
}
|
|
|
|
return t; // 충돌이 일어남
|
|
}
|
|
|
|
static inline bool imp_triangle_collide_edge( const K3DVector& p1, const K3DVector& p2, const K3DVector& p3,
|
|
const K3DVector& e1, const K3DVector& e2 )
|
|
{
|
|
K3DVector n, t1, t2;
|
|
|
|
vec_cross_vec( n, p2 - p1, p3 - p1 );
|
|
t1 = e1 - p1; t2 = e2 - p1;
|
|
|
|
// 모두 plane의 같은 방향에 있으면...
|
|
if ( vec_dot_vec( n, t1 ) * vec_dot_vec( n, t2 ) > 0 )
|
|
return false;
|
|
|
|
|
|
// 삼각뿔을 만들자~!
|
|
|
|
// 새로운 평면 1에 대해...(e1, p1, p2)
|
|
// t1 = e1 - p1; t2 = e2 - p1;
|
|
vec_cross_vec( n, p2 - p1, t1 );
|
|
if ( vec_dot_vec( n, t2 ) > 0 ) return false;
|
|
// 새로운 평면 2에 대해...(e1, p2, p3)
|
|
t1 = e1 - p2; t2 = e2 - p2;
|
|
vec_cross_vec( n, p3 - p2, t1 );
|
|
if ( vec_dot_vec( n, t2 ) > 0 ) return false;
|
|
// 새로운 평면 3에 대해...(e1, p3, p1)
|
|
t1 = e1 - p3; t2 = e2 - p3;
|
|
vec_cross_vec( n, p1 - p3, t1 );
|
|
if ( vec_dot_vec( n, t2 ) > 0 ) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
static inline bool imp_quad_collide_edge(
|
|
const K3DVector& p1, const K3DVector& p2, const K3DVector& p3, const K3DVector& p4,
|
|
const K3DVector& e1, const K3DVector& e2 )
|
|
{
|
|
K3DVector n, t1, t2;
|
|
|
|
vec_cross_vec( n, p2 - p1, p3 - p1 );
|
|
t1 = e1 - p1; t2 = e2 - p1;
|
|
|
|
// 모두 plane의 같은 방향에 있으면...
|
|
if ( vec_dot_vec( n, t1 ) * vec_dot_vec( n, t2 ) > 0 )
|
|
return false;
|
|
|
|
|
|
// 사각뿔을 만들자~!
|
|
|
|
// 새로운 평면 1에 대해...(e1, p1, p2)
|
|
// t1 = e1 - p1; t2 = e2 - p1;
|
|
vec_cross_vec( n, p2 - p1, t1 );
|
|
if ( vec_dot_vec( n, t2 ) < 0 ) return false;
|
|
|
|
// 새로운 평면 2에 대해...(e1, p2, p3)
|
|
t1 = e1 - p2; t2 = e2 - p2;
|
|
vec_cross_vec( n, p3 - p2, t1 );
|
|
if ( vec_dot_vec( n, t2 ) < 0 ) return false;
|
|
|
|
// 새로운 평면 3에 대해...(e1, p3, p4)
|
|
t1 = e1 - p3; t2 = e2 - p3;
|
|
vec_cross_vec( n, p4 - p3, t1 );
|
|
if ( vec_dot_vec( n, t2 ) < 0 ) return false;
|
|
|
|
// 새로운 평면 4에 대해...(e1, p4, p1)
|
|
t1 = e1 - p4; t2 = e2 - p4;
|
|
vec_cross_vec( n, p1 - p4, t1 );
|
|
if ( vec_dot_vec( n, t2 ) < 0 ) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
static inline bool imp_triangle_collide_triangle( const K3DVector &a1, const K3DVector &a2, const K3DVector &a3,
|
|
const K3DVector &b1, const K3DVector &b2, const K3DVector &b3 )
|
|
{
|
|
K3DVector u, v, w, n, t1, t2, t3;
|
|
float d1, d2, d3;
|
|
|
|
make_vector(u, a1, a2); make_vector(v, a1, a3); vec_cross_vec(n, u, v);
|
|
make_vector(t1, a1, b1); make_vector(t2, a1, b2); make_vector(t3, a1, b3);
|
|
d1 = vec_dot_vec(n, t1); d2 = vec_dot_vec(n, t2); d3 = vec_dot_vec(n, t3);
|
|
if((d1 > 0 && d2 > 0 && d3 > 0) || (d1 < 0 && d2 < 0 && d3 < 0)) return false;
|
|
|
|
// a->b의 Edge에 대해서 검사
|
|
make_vector(v, a2, a3); make_vector(w, a3, a1);
|
|
// b1b2와 Triangle a에 대해서 검사
|
|
if(d1*d2 <= 0) { // Edge 각점의 plane에 대한 위치 검사
|
|
make_vector(t1, a1, b1); make_vector(t2, a1, b2);
|
|
vec_cross_vec(n, u, t1); if(vec_dot_vec(n, t2) > 0) goto FAIL_B1B2_TRI_A; // (b1, a1, a2)
|
|
make_vector(t1, a2, b1); make_vector(t2, a2, b2);
|
|
vec_cross_vec(n, v, t1); if(vec_dot_vec(n, t2) > 0) goto FAIL_B1B2_TRI_A; // (b1, a2, a3)
|
|
make_vector(t1, a3, b1); make_vector(t2, a3, b2);
|
|
vec_cross_vec(n, w, t1); if(vec_dot_vec(n, t2) > 0) goto FAIL_B1B2_TRI_A; // (b1, a3, a1)
|
|
return true;
|
|
}
|
|
|
|
FAIL_B1B2_TRI_A:
|
|
|
|
// b2b3와 Triangle a에 대해서 검사
|
|
if(d2*d3 <= 0) {
|
|
make_vector(t1, a1, b2); make_vector(t2, a1, b3); vec_cross_vec(n, u, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_B2B3_TRI_A; // (b2, a1, a2)
|
|
make_vector(t1, a2, b2); make_vector(t2, a2, b3); vec_cross_vec(n, v, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_B2B3_TRI_A; // (b2, a2, a3)
|
|
make_vector(t1, a3, b2); make_vector(t2, a3, b3); vec_cross_vec(n, w, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_B2B3_TRI_A; // (b2, a3, a1)
|
|
return true;
|
|
}
|
|
|
|
FAIL_B2B3_TRI_A:
|
|
|
|
// b3b1와 Triangle a에 대해서 검사
|
|
if(d3*d1 <= 0) {
|
|
make_vector(t1, a1, b3); make_vector(t2, a1, b1); vec_cross_vec(n, u, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_B3B1_TRI_A; // (b3, a1, a2)
|
|
make_vector(t1, a2, b3); make_vector(t2, a2, b1); vec_cross_vec(n, v, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_B3B1_TRI_A; // (b3, a2, a3)
|
|
make_vector(t1, a3, b3); make_vector(t2, a3, b1); vec_cross_vec(n, w, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_B3B1_TRI_A; // (b3, a3, a1)
|
|
return true;
|
|
}
|
|
|
|
FAIL_B3B1_TRI_A:
|
|
|
|
make_vector(u, b1, b2); make_vector(v, b1, b3); vec_cross_vec(n, u, v);
|
|
make_vector(t1, b1, a1); make_vector(t2, b1, a2); make_vector(t3, b1, a3);
|
|
d1 = vec_dot_vec(n, t1); d2 = vec_dot_vec(n, t2); d3 = vec_dot_vec(n, t3);
|
|
|
|
// b->a의 Edge에 대해서 검사
|
|
make_vector(v, b2, b3); make_vector(w, b3, b1);
|
|
// a1a2와 Triangle b에 대해서 검사
|
|
if(d1*d2 <= 0) {
|
|
make_vector(t1, b1, a1); make_vector(t2, b1, a2); vec_cross_vec(n, u, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A1A2_TRI_B; // (a1, b1, b2)
|
|
make_vector(t1, b2, a1); make_vector(t2, b2, a2); vec_cross_vec(n, v, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A1A2_TRI_B; // (a1, b2, b3)
|
|
make_vector(t1, b3, a1); make_vector(t2, b3, a2); vec_cross_vec(n, w, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A1A2_TRI_B; // (a1, b3, b1)
|
|
return true;
|
|
}
|
|
|
|
FAIL_A1A2_TRI_B:
|
|
|
|
// a2a3와 Triangle b에 대해서 검사
|
|
if(d2*d3 <= 0) {
|
|
make_vector(t1, b1, a2); make_vector(t2, b1, a3); vec_cross_vec(n, u, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A2A3_TRI_B; // (a2, b1, b2)
|
|
make_vector(t1, b2, a2); make_vector(t2, b2, a3); vec_cross_vec(n, v, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A2A3_TRI_B; // (a2, b2, b3)
|
|
make_vector(t1, b3, a2); make_vector(t2, b3, a3); vec_cross_vec(n, w, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A2A3_TRI_B; // (a2, b3, b1)
|
|
return true;
|
|
}
|
|
|
|
FAIL_A2A3_TRI_B:
|
|
|
|
// a3a1와 Triangle b에 대해서 검사
|
|
if(d3*d1 <= 0) {
|
|
make_vector(t1, b1, a3); make_vector(t2, b1, a1); vec_cross_vec(n, u, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A3A1_TRI_B; // (a3, b1, b2)
|
|
make_vector(t1, b2, a3); make_vector(t2, b2, a1); vec_cross_vec(n, v, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A3A1_TRI_B; // (a3, b2, b3)
|
|
make_vector(t1, b3, a3); make_vector(t2, b3, a1); vec_cross_vec(n, w, t1);
|
|
if (vec_dot_vec(n, t2) > 0) goto FAIL_A3A1_TRI_B; // (a3, b3, b1)
|
|
return true;
|
|
}
|
|
|
|
FAIL_A3A1_TRI_B:
|
|
|
|
return false;
|
|
}
|
|
|
|
static inline bool imp_triangle_collide_sphere( const K3DVector &p1, const K3DVector &p2, const K3DVector &p3,
|
|
const K3DVector &s, float r )
|
|
{
|
|
/*
|
|
essentials :
|
|
the shortest distance of triangle from sphere(? konglish 공부좀 할걸...)
|
|
*/
|
|
|
|
K3DVector u, v, w;
|
|
K3DVector pl, cp1, cp2, cp3;
|
|
|
|
// sphere is uppon triangle's plane..? --> means never collide
|
|
make_vector(u, p1, p2); make_vector(v, p2, p3);
|
|
vec_cross_vec(pl, u, v); vec_normalize(pl);
|
|
float pld = -(pl.x*p1.x+pl.y*p1.y+pl.z*p1.z);
|
|
float dis = pl.x*s.x+pl.y*s.y+pl.z*s.z+pld;
|
|
if(abs(dis) > r)
|
|
return false;
|
|
|
|
// sphere's center is in triangle..? --> means certainly collide
|
|
// p1p2를 통하며 plane p1p2p3에 수직인 평면에 대해 검사한다
|
|
vec_cross_vec(cp1, pl, u); float d1 = vec_dot_vec(cp1, p1);
|
|
vec_cross_vec(cp2, pl, v); float d2 = vec_dot_vec(cp2, p2);
|
|
make_vector(w, p3, p1); vec_cross_vec(cp3, pl, w); float d3 = vec_dot_vec(cp3, p3);
|
|
if ((vec_dot_vec(cp1, s) - d1)*(vec_dot_vec(cp1, p3) - d1) >= 0 &&
|
|
(vec_dot_vec(cp2, s) - d2)*(vec_dot_vec(cp2, p1) - d2) >= 0 &&
|
|
(vec_dot_vec(cp3, s) - d3)*(vec_dot_vec(cp3, p2) - d3) >= 0 )
|
|
return true;
|
|
|
|
float temp, shdis = vec_distance(p1, s);
|
|
// check each side is acute angle & check distance(if it's acute angle.) --> means the shortest distance
|
|
temp = vec_distance(p2, s); shdis = shdis > temp ? temp : shdis;
|
|
temp = vec_distance(p3, s); shdis = shdis > temp ? temp : shdis;
|
|
|
|
float dot, cdot;
|
|
make_vector(u, s, p1); make_vector(v, p1, p2); dot = vec_dot_vec(u, v);
|
|
make_vector(u, s, p2); make_vector(v, p2, p1); cdot = vec_dot_vec(u, v);
|
|
if(dot*cdot > 0) {
|
|
make_vector(u, s, p1); make_vector(v, p1, p2);
|
|
temp = vec_distance(u, v)/vec_distance(p2, s)/2;
|
|
shdis = shdis > temp ? temp : shdis;
|
|
}
|
|
|
|
make_vector(u, s, p2); make_vector(v, p2, p3); dot = vec_dot_vec(u, v);
|
|
make_vector(u, s, p3); make_vector(v, p3, p2); cdot = vec_dot_vec(u, v);
|
|
if(dot*cdot > 0) {
|
|
make_vector(u, s, p2); make_vector(v, p2, p3);
|
|
temp = vec_distance(u, v)/vec_distance(p3, s)/2;
|
|
shdis = shdis > temp ? temp : shdis;
|
|
}
|
|
|
|
make_vector(u, s, p3); make_vector(v, p3, p1); dot = vec_dot_vec(u, v);
|
|
make_vector(u, s, p1); make_vector(v, p1, p3); cdot = vec_dot_vec(u, v);
|
|
if(dot*cdot > 0) {
|
|
make_vector(u, s, p3); make_vector(v, p3, p1);
|
|
temp = vec_distance(u, v)/vec_distance(p1, s)/2;
|
|
shdis = shdis > temp ? temp : shdis;
|
|
}
|
|
|
|
if(shdis <= r)
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
// { [sonador][7.1.7]충돌 처리 개선(Swept Shpere Collision Detection)
|
|
static inline bool imp_triangle_collide_swept_unit_sphere(
|
|
const K3DVector& p1
|
|
, const K3DVector& p2
|
|
, const K3DVector& p3
|
|
, const K3DVector& base
|
|
, const K3DVector& velocity
|
|
, K3DVector& collision
|
|
, float& t )
|
|
{
|
|
float t0, t1;
|
|
bool embedded = false;
|
|
|
|
// find plane normal vector
|
|
K3DVector plane_normal, edge1, edge2;
|
|
make_vector( edge1, p1, p2 );
|
|
make_vector( edge2, p2, p3 );
|
|
vec_cross_vec( plane_normal, edge1, edge2 );
|
|
vec_normalize( plane_normal );
|
|
|
|
// find t0, t1 from signed distance
|
|
float signed_distance = vec_distance_from_plane( plane_normal, p1, base );
|
|
float determinant = vec_dot_vec( plane_normal, velocity );
|
|
if( determinant == 0.0f )
|
|
{
|
|
if( ::fabs( signed_distance ) >= 1.0f )
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
t0 = 0.0f; t1 = 1.0f;
|
|
embedded = true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
t0 = ( 1.0f - signed_distance ) / determinant;
|
|
t1 = ( -1.0f - signed_distance ) / determinant;
|
|
|
|
if( t0 > t1 )
|
|
{
|
|
float temp = t1;
|
|
t1 = t0;
|
|
t0 = temp;
|
|
}
|
|
|
|
if( t0 > 1.0f || t1 < 0.0f )
|
|
return false;
|
|
|
|
// normailize
|
|
if( t0 < 0.0f ) t0 = 0.0f;
|
|
if( t1 < 0.0f ) t1 = 0.0f;
|
|
if( t0 > 1.0f ) t0 = 1.0f;
|
|
if( t1 > 1.0f ) t1 = 1.0f;
|
|
}
|
|
|
|
bool found = false;
|
|
|
|
// collision detection with triangle
|
|
if( !embedded )
|
|
{
|
|
K3DVector plane_intersection_point = base - plane_normal + (float)t0 * velocity;
|
|
if( check_point_in_triangle( plane_intersection_point, p1, p2, p3 ) )
|
|
{
|
|
found = true;
|
|
t = t0;
|
|
collision = plane_intersection_point;
|
|
}
|
|
}
|
|
|
|
// collision detection with vertices and edges...
|
|
if( found == false )
|
|
{
|
|
float velocity_squared_length = velocity.SquareMagnitude( );
|
|
float a, b, c;
|
|
float root;
|
|
|
|
// check against points...
|
|
a = velocity_squared_length;
|
|
|
|
// check vertex p1
|
|
b = 2.0f * ( vec_dot_vec( velocity, ( base - p1 ) ) );
|
|
c = ( p1 - base ).SquareMagnitude( ) - 1.0f;
|
|
if( get_lowest_root( a, b, c, t, root ) )
|
|
{
|
|
found = true;
|
|
t = root;
|
|
collision = p1;
|
|
}
|
|
|
|
// check vertex p2
|
|
b = 2.0f * ( vec_dot_vec( velocity, ( base - p2 ) ) );
|
|
c = ( p2 - base ).SquareMagnitude( ) - 1.0f;
|
|
if( get_lowest_root( a, b, c, t, root ) )
|
|
{
|
|
found = true;
|
|
t = root;
|
|
collision = p2;
|
|
}
|
|
|
|
// check vertex p3
|
|
b = 2.0f * ( vec_dot_vec( velocity, ( base - p3 ) ) );
|
|
c = ( p3 - base ).SquareMagnitude( ) - 1.0f;
|
|
if( get_lowest_root( a, b, c, t, root ) )
|
|
{
|
|
found = true;
|
|
t = root;
|
|
collision = p3;
|
|
}
|
|
|
|
// check against edges...
|
|
K3DVector edge3;
|
|
make_vector( edge3, p3, p1 );
|
|
|
|
// check edge: p1 -> p2
|
|
K3DVector& edge = edge1;
|
|
K3DVector base_to_vertex = p1 - base;
|
|
float edge_squared_length = edge.SquareMagnitude( );
|
|
float edge_dot_velocity = vec_dot_vec( edge, velocity );
|
|
float edge_dot_base_to_vertex = vec_dot_vec( edge, base_to_vertex );
|
|
float velocity_dot_base_to_vertex = vec_dot_vec( velocity, base_to_vertex );
|
|
a = edge_squared_length * -velocity_squared_length + edge_dot_velocity * edge_dot_velocity;
|
|
b = edge_squared_length * ( 2.0f * velocity_dot_base_to_vertex ) - 2.0f * edge_dot_velocity * edge_dot_base_to_vertex;
|
|
c = edge_squared_length * ( 1.0f - base_to_vertex.SquareMagnitude( ) ) + edge_dot_base_to_vertex * edge_dot_base_to_vertex;
|
|
if( get_lowest_root( a, b, c, t, root ) )
|
|
{
|
|
float f0 = ( edge_dot_velocity * root - edge_dot_base_to_vertex ) / edge_squared_length;
|
|
if( f0 >= 0.0f && f0 <= 1.0f )
|
|
{
|
|
found = true;
|
|
t = root;
|
|
collision = p1 + f0 * edge;
|
|
}
|
|
}
|
|
|
|
// check edge: p2 -> p3
|
|
edge = edge2;
|
|
base_to_vertex = p2 - base;
|
|
edge_squared_length = edge.SquareMagnitude( );
|
|
edge_dot_velocity = vec_dot_vec( edge, velocity );
|
|
edge_dot_base_to_vertex = vec_dot_vec( edge, base_to_vertex );
|
|
velocity_dot_base_to_vertex = vec_dot_vec( velocity, base_to_vertex );
|
|
a = edge_squared_length * -velocity_squared_length + edge_dot_velocity * edge_dot_velocity;
|
|
b = edge_squared_length * ( 2.0f * velocity_dot_base_to_vertex ) - 2.0f * edge_dot_velocity * edge_dot_base_to_vertex;
|
|
c = edge_squared_length * ( 1.0f - base_to_vertex.SquareMagnitude( ) ) + edge_dot_base_to_vertex * edge_dot_base_to_vertex;
|
|
if( get_lowest_root( a, b, c, t, root ) )
|
|
{
|
|
float f0 = ( edge_dot_velocity * root - edge_dot_base_to_vertex ) / edge_squared_length;
|
|
if( f0 >= 0.0f && f0 <= 1.0f )
|
|
{
|
|
found = true;
|
|
t = root;
|
|
collision = p2 + f0 * edge;
|
|
}
|
|
}
|
|
|
|
// check edge: p3 -> p1
|
|
edge = edge3;
|
|
base_to_vertex = p3 - base;
|
|
edge_squared_length = edge.SquareMagnitude( );
|
|
edge_dot_velocity = vec_dot_vec( edge, velocity );
|
|
edge_dot_base_to_vertex = vec_dot_vec( edge, base_to_vertex );
|
|
velocity_dot_base_to_vertex = vec_dot_vec( velocity, base_to_vertex );
|
|
a = edge_squared_length * -velocity_squared_length + edge_dot_velocity * edge_dot_velocity;
|
|
b = edge_squared_length * ( 2.0f * velocity_dot_base_to_vertex ) - 2.0f * edge_dot_velocity * edge_dot_base_to_vertex;
|
|
c = edge_squared_length * ( 1.0f - base_to_vertex.SquareMagnitude( ) ) + edge_dot_base_to_vertex * edge_dot_base_to_vertex;
|
|
if( get_lowest_root( a, b, c, t, root ) )
|
|
{
|
|
float f0 = ( edge_dot_velocity * root - edge_dot_base_to_vertex ) / edge_squared_length;
|
|
if( f0 >= 0.0f && f0 <= 1.0f )
|
|
{
|
|
found = true;
|
|
t = root;
|
|
collision = p3 + f0 * edge;
|
|
}
|
|
}
|
|
}
|
|
|
|
return found;
|
|
}
|
|
// }
|
|
|
|
static inline bool imp_rect_collide_rect( const K3DVector *pa, const K3DVector *pb )
|
|
{
|
|
int i;
|
|
K3DVector cv, ov;
|
|
cv.z = 0; ov.z = 0;
|
|
|
|
// Cube1 -> Cube2
|
|
// front back
|
|
cv.x = - (pa[0].y - pa[1].y);
|
|
cv.y = (pa[0].x - pa[1].x);
|
|
for(i = 0; i < 4; i++) {
|
|
ov.x = pb[i].x - pa[0].x;
|
|
ov.y = pb[i].y - pa[0].y;
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_FRONT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_FRONT:
|
|
|
|
cv.x = - (pa[1].y - pa[2].y);
|
|
cv.y = (pa[1].x - pa[2].x);
|
|
for(i = 0; i < 4; i++) {
|
|
ov.x = pb[i].x - pa[1].x;
|
|
ov.y = pb[i].y - pa[1].y;
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_BACK;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_BACK:
|
|
|
|
// left right
|
|
cv.x = - (pa[2].y - pa[3].y);
|
|
cv.y = (pa[2].x - pa[3].x);
|
|
for(i = 0; i < 4; i++) {
|
|
ov.x = pb[i].x - pa[2].x;
|
|
ov.y = pb[i].y - pa[2].y;
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_LEFT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_LEFT:
|
|
|
|
cv.x = - (pa[3].y - pa[0].y);
|
|
cv.y = (pa[3].x - pa[0].x);
|
|
for(i = 0; i < 4; i++) {
|
|
ov.x = pb[i].x - pa[3].x;
|
|
ov.y = pb[i].y - pa[3].y;
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_RIGHT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_RIGHT:
|
|
|
|
|
|
|
|
// Cube2 -> Cube1
|
|
// front back
|
|
cv.x = - (pb[0].y - pb[1].y);
|
|
cv.y = (pb[0].x - pb[1].x);
|
|
cv.z = 0;
|
|
for(i = 0; i < 4; i++) {
|
|
ov.x = pa[i].x - pb[0].x;
|
|
ov.y = pa[i].y - pb[0].y;
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_FRONT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_FRONT:
|
|
|
|
cv.x = - (pb[1].y - pb[2].y);
|
|
cv.y = (pb[1].x - pb[2].x);
|
|
for(i = 0; i < 4; i++) {
|
|
ov.x = pa[i].x - pb[1].x;
|
|
ov.y = pa[i].y - pb[1].y;
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_BACK;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_BACK:
|
|
|
|
// left right
|
|
cv.x = - (pb[2].y - pb[3].y);
|
|
cv.y = (pb[2].x - pb[3].x);
|
|
for(i = 0; i < 4; i++) {
|
|
ov.x = pa[i].x - pb[2].x;
|
|
ov.y = pa[i].y - pb[2].y;
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_LEFT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_LEFT:
|
|
|
|
cv.x = - (pb[3].y - pb[0].y);
|
|
cv.y = (pb[3].x - pb[0].x);
|
|
for(i = 0; i < 4; i++) {
|
|
ov.x = pa[i].x - pb[3].x;
|
|
ov.y = pa[i].y - pb[3].y;
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_RIGHT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_RIGHT:
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
static inline bool imp_point_collide_cube( const K3DVector& p,
|
|
float minx, float maxx,
|
|
float miny, float maxy,
|
|
float minz, float maxz )
|
|
{
|
|
// is point included in cube..?
|
|
if( p.x > minx && p.y > miny && p.z > minz && p.x < maxx && p.y < maxy && p.z < maxz )
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
static inline bool imp_edge_collide_cube( const K3DVector& e1, const K3DVector& e2,
|
|
float minx, float maxx,
|
|
float miny, float maxy,
|
|
float minz, float maxz )
|
|
{
|
|
// is line included in cube..?
|
|
if( e1.x > minx && e1.y > miny && e1.z > minz && e1.x < maxx && e1.y < maxy && e1.z < maxz ) return true;
|
|
if( e2.x > minx && e2.y > miny && e2.z > minz && e2.x < maxx && e2.y < maxy && e2.z < maxz ) return true;
|
|
|
|
// front
|
|
if(imp_quad_collide_edge(get_cube_vertex(3, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(2, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(1, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(0, minx, maxx, miny, maxy, minz, maxz),
|
|
e1, e2) == true) return true;
|
|
|
|
// back
|
|
if(imp_quad_collide_edge(get_cube_vertex(6, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(7, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(4, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(5, minx, maxx, miny, maxy, minz, maxz),
|
|
e1, e2) == true) return true;
|
|
|
|
// left
|
|
if(imp_quad_collide_edge(get_cube_vertex(2, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(6, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(5, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(1, minx, maxx, miny, maxy, minz, maxz),
|
|
e1, e2) == true) return true;
|
|
|
|
// right
|
|
if(imp_quad_collide_edge(get_cube_vertex(7, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(3, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(0, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(4, minx, maxx, miny, maxy, minz, maxz),
|
|
e1, e2) == true) return true;
|
|
|
|
// top
|
|
if(imp_quad_collide_edge(get_cube_vertex(7, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(6, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(2, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(3, minx, maxx, miny, maxy, minz, maxz),
|
|
e1, e2) == true) return true;
|
|
|
|
// bottom
|
|
if(imp_quad_collide_edge(get_cube_vertex(0, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(1, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(5, minx, maxx, miny, maxy, minz, maxz),
|
|
get_cube_vertex(4, minx, maxx, miny, maxy, minz, maxz),
|
|
e1, e2) == true) return true;
|
|
|
|
return false;
|
|
}
|
|
|
|
static inline bool imp_triangle_collide_cube( const K3DVector& p1, const K3DVector& p2, const K3DVector& p3,
|
|
float minx, float maxx,
|
|
float miny, float maxy,
|
|
float minz, float maxz )
|
|
{
|
|
//assert((minx <= maxx) && (miny <= maxy) && (minz <= maxz));
|
|
|
|
// Triangle -> Cube
|
|
// top bottom
|
|
if ( p1.z < minz && p2.z < minz && p3.z < minz )
|
|
return false;
|
|
|
|
if ( p1.z > maxz && p2.z > maxz && p3.z > maxz )
|
|
return false;
|
|
|
|
// left right
|
|
if ( p1.x < minx && p2.x < minx && p3.x < minx )
|
|
return false;
|
|
|
|
if ( p1.x > maxx && p2.x > maxx && p3.x > maxx )
|
|
return false;
|
|
|
|
// front back
|
|
if ( p1.y < miny && p2.y < miny && p3.y < miny )
|
|
return false;
|
|
|
|
if ( p1.y > maxy && p2.y > maxy && p3.y > maxy )
|
|
return false;
|
|
|
|
// to do : Optimize
|
|
// 대략... min 값에 대해서 작으면 그 후는 계산할 필요가 없고
|
|
// max 값에 대해서 크면 역시 계산할 필요가 없음
|
|
|
|
// Cube -> Triangle
|
|
K3DVector cp, fcp, v1, v2;
|
|
K3DVector v;
|
|
float d; // cp X (vertex) - d = 0
|
|
int i;
|
|
|
|
// Front
|
|
make_vector(v1, p1, p2);
|
|
make_vector(v2, p2, p3);
|
|
vec_cross_vec(cp, v1, v2);
|
|
d = vec_dot_vec(cp, p1);
|
|
for (i = 0; i < 8; i++) {
|
|
v = get_cube_vertex( i, minx, maxx, miny, maxy, minz, maxz );
|
|
if(vec_dot_vec(cp, v) <= float(d)) break;
|
|
}
|
|
if (i == 8) return false;
|
|
|
|
// Back
|
|
cp.x = -cp.x; cp.y = -cp.y; cp.z = -cp.z;
|
|
d = vec_dot_vec(cp, p1); fcp = cp; // means : front side cross product
|
|
for (i = 0; i < 8; i++) {
|
|
v = get_cube_vertex( i, minx, maxx, miny, maxy, minz, maxz );
|
|
if(vec_dot_vec(cp, v) <= float(d)) break;
|
|
}
|
|
if (i == 8) return false;
|
|
|
|
float td;
|
|
// Line 1
|
|
make_vector(v1, p1, p2);
|
|
vec_cross_vec(cp, fcp, v1);
|
|
d = vec_dot_vec(cp, p1);
|
|
td = vec_dot_vec(cp, p3) - d;
|
|
for (i = 0; i < 8; i++) {
|
|
v = get_cube_vertex( i, minx, maxx, miny, maxy, minz, maxz );
|
|
if((vec_dot_vec(cp, v) - float(d))*float(td) <= float(0.f)) break;
|
|
//if(vec_dot_vtx(cp, v) <= float(d)) break;
|
|
}
|
|
if (i == 8) return false;
|
|
|
|
// Line 2
|
|
make_vector(v1, p2, p3);
|
|
vec_cross_vec(cp, fcp, v1);
|
|
d = vec_dot_vec(cp, p2);
|
|
td = vec_dot_vec(cp, p1) - d;
|
|
for(i = 0; i < 8; i++) {
|
|
v = get_cube_vertex( i, minx, maxx, miny, maxy, minz, maxz );
|
|
if((vec_dot_vec(cp, v) - float(d))*float(td) <= float(0.f)) break;
|
|
//if(vec_dot_vtx(cp, v) <= float(d)) break;
|
|
}
|
|
if(i == 8) return false;
|
|
|
|
// Line 3
|
|
make_vector(v1, p3, p1);
|
|
vec_cross_vec(cp, fcp, v1);
|
|
d = vec_dot_vec(cp, p3);
|
|
td = vec_dot_vec(cp, p2) - d;
|
|
for(i = 0; i < 8; i++) {
|
|
v = get_cube_vertex( i, minx, maxx, miny, maxy, minz, maxz );
|
|
if((vec_dot_vec(cp, v) - float(d))*float(td) <= float(0.f)) break;
|
|
//if(vec_dot_vtx(cp, v) <= float(d)) break;
|
|
}
|
|
if(i == 8) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
// rotated cube vs triangle
|
|
static inline bool imp_triangle_collide_rotcube( const K3DVector* triangle, const K3DVector* cube )
|
|
{
|
|
int i;
|
|
K3DVector cv, ov;
|
|
|
|
// Triangle -> Cube
|
|
// top bottom
|
|
make_vector(cv, cube[3], cube[0]);
|
|
for(i = 0; i < 3; i++) {
|
|
make_vector(ov, cube[0], triangle[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto TRI_CUBE_TOP;
|
|
}
|
|
return false;
|
|
TRI_CUBE_TOP:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 3; i++) {
|
|
make_vector(ov, cube[3], triangle[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto TRI_CUBE_BOTTOM;
|
|
}
|
|
return false;
|
|
TRI_CUBE_BOTTOM:
|
|
|
|
// left right
|
|
make_vector(cv, cube[1], cube[0]);
|
|
for(i = 0; i < 3; i++) {
|
|
make_vector(ov, cube[0], triangle[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto TRI_CUBE_LEFT;
|
|
}
|
|
return false;
|
|
TRI_CUBE_LEFT:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 3; i++) {
|
|
make_vector(ov, cube[1], triangle[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto TRI_CUBE_RIGHT;
|
|
}
|
|
return false;
|
|
TRI_CUBE_RIGHT:
|
|
|
|
// front back
|
|
make_vector(cv, cube[5], cube[0]);
|
|
for(i = 0; i < 3; i++) {
|
|
make_vector(ov, cube[0], triangle[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto TRI_CUBE_FRONT;
|
|
}
|
|
return false;
|
|
TRI_CUBE_FRONT:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 3; i++) {
|
|
make_vector(ov, cube[5], triangle[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto TRI_CUBE_BACK;
|
|
}
|
|
return false;
|
|
TRI_CUBE_BACK:
|
|
|
|
|
|
// Cube -> Triangle
|
|
K3DVector cp, fcp, v1, v2;
|
|
float d; // cp X (vertex) - d = 0
|
|
|
|
// Front
|
|
make_vector(v1, triangle[0], triangle[1]);
|
|
make_vector(v2, triangle[1], triangle[2]);
|
|
vec_cross_vec(cp, v1, v2);
|
|
d = vec_dot_vec(cp, triangle[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_FRONT_CUBE;
|
|
}
|
|
return false;
|
|
TRI_FRONT_CUBE:
|
|
|
|
fcp = cp; // means : front side cross product
|
|
|
|
// Back
|
|
cp.x = -cp.x; cp.y = -cp.y; cp.z = -cp.z;
|
|
d = vec_dot_vec(cp, triangle[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_BACK_CUBE;
|
|
}
|
|
return false;
|
|
TRI_BACK_CUBE:
|
|
|
|
// Edge 1
|
|
make_vector(v1, triangle[0], triangle[1]);
|
|
vec_cross_vec(cp, v1, fcp);
|
|
d = vec_dot_vec(cp, triangle[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_EDGE1_CUBE;
|
|
}
|
|
return false;
|
|
TRI_EDGE1_CUBE:
|
|
|
|
// Edge 2
|
|
make_vector(v1, triangle[1], triangle[2]);
|
|
vec_cross_vec(cp, v1, fcp);
|
|
d = vec_dot_vec(cp, triangle[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_EDGE2_CUBE;
|
|
}
|
|
return false;
|
|
TRI_EDGE2_CUBE:
|
|
|
|
// Edge 3
|
|
make_vector(v1, triangle[2], triangle[3]);
|
|
vec_cross_vec(cp, v1, fcp);
|
|
d = vec_dot_vec(cp, triangle[2]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_EDGE3_CUBE;
|
|
}
|
|
return false;
|
|
TRI_EDGE3_CUBE:
|
|
|
|
return true;
|
|
}
|
|
|
|
// rotated cube vs quad
|
|
static inline bool imp_rotcube_collide_rotcube( const K3DVector* cube1, const K3DVector* cube2 )
|
|
{
|
|
int i;
|
|
K3DVector cv, ov;
|
|
|
|
// Cube1 -> Cube2
|
|
// top bottom
|
|
make_vector(cv, cube1[3], cube1[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube1[0], cube2[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_TOP;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_TOP:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube1[3], cube2[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_BOTTOM;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_BOTTOM:
|
|
|
|
// left right
|
|
make_vector(cv, cube1[1], cube1[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube1[0], cube2[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_LEFT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_LEFT:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube1[1], cube2[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_RIGHT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_RIGHT:
|
|
|
|
// front back
|
|
make_vector(cv, cube1[5], cube1[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube1[0], cube2[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_FRONT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_FRONT:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube1[5], cube2[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE2_CUBE1_BACK;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_BACK:
|
|
|
|
|
|
// Cube2 -> Cube1
|
|
// top bottom
|
|
make_vector(cv, cube2[3], cube2[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube2[0], cube1[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_TOP;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_TOP:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube2[3], cube1[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_BOTTOM;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_BOTTOM:
|
|
|
|
// left right
|
|
make_vector(cv, cube2[1], cube2[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube2[0], cube1[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_LEFT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_LEFT:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube2[1], cube1[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_RIGHT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_RIGHT:
|
|
|
|
// front back
|
|
make_vector(cv, cube2[5], cube2[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube2[0], cube1[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_FRONT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_FRONT:
|
|
|
|
cv.x = -cv.x; cv.y = -cv.y; cv.z = -cv.z;
|
|
for(i = 0; i < 8; i++) {
|
|
make_vector(ov, cube2[5], cube1[i]);
|
|
if(vec_dot_vec(cv, ov) <= 0) goto CUBE1_CUBE2_BACK;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_BACK:
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// nonuniform cube vs point
|
|
static inline bool imp_point_collide_nonuniformcube( const K3DVector* point, const K3DVector *cube)
|
|
{
|
|
K3DVector cv, ov;
|
|
|
|
// Triangle -> Cube
|
|
// top bottom
|
|
vec_cross_vec(cv, cube[0] - cube[1], cube[5] - cube[0]);
|
|
ov = point[0] - cube[0];
|
|
if(vec_dot_vec(cv, ov) < 0) return false;
|
|
|
|
vec_cross_vec(cv, cube[2] - cube[3], cube[7] - cube[2]);
|
|
ov = point[0] - cube[3];
|
|
if(vec_dot_vec(cv, ov) < 0) return false;
|
|
|
|
// left right
|
|
vec_cross_vec(cv, cube[0] - cube[5], cube[3] - cube[0]);
|
|
ov = point[0] - cube[0];
|
|
if(vec_dot_vec(cv, ov) < 0) return false;
|
|
|
|
vec_cross_vec(cv, cube[1] - cube[2], cube[4] - cube[1]);
|
|
ov = point[0] - cube[1];
|
|
if(vec_dot_vec(cv, ov) < 0) return false;
|
|
|
|
// front back
|
|
vec_cross_vec(cv, cube[0] - cube[3], cube[1] - cube[0]);
|
|
ov = point[0] - cube[0];
|
|
if(vec_dot_vec(cv, ov) < 0) return false;
|
|
|
|
vec_cross_vec(cv, cube[5] - cube[4], cube[6] - cube[5]);
|
|
ov = point[0] - cube[5];
|
|
if(vec_dot_vec(cv, ov) < 0) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
static inline bool imp_edge_collide_nonuniformcube( const K3DVector* edge, const K3DVector *cube )
|
|
{
|
|
// front
|
|
if(imp_quad_collide_edge(cube[0], cube[1], cube[2], cube[3],
|
|
edge[0], edge[1]) == true) return true;
|
|
|
|
// back
|
|
if(imp_quad_collide_edge(cube[4], cube[5], cube[6], cube[7],
|
|
edge[0], edge[1]) == true) return true;
|
|
|
|
// left
|
|
if(imp_quad_collide_edge(cube[0], cube[3], cube[6], cube[5],
|
|
edge[0], edge[1]) == true) return true;
|
|
|
|
// right
|
|
if(imp_quad_collide_edge(cube[1], cube[4], cube[7], cube[2],
|
|
edge[0], edge[1]) == true) return true;
|
|
|
|
// top
|
|
if(imp_quad_collide_edge(cube[0], cube[5], cube[4], cube[1],
|
|
edge[0], edge[1]) == true) return true;
|
|
|
|
// bottom
|
|
if(imp_quad_collide_edge(cube[2], cube[7], cube[6], cube[3],
|
|
edge[0], edge[1]) == true) return true;
|
|
|
|
return imp_point_collide_nonuniformcube(&edge[0], cube) | imp_point_collide_nonuniformcube(&edge[1], cube);
|
|
}
|
|
|
|
// nonuniform cube vs triangle
|
|
static inline bool imp_triangle_collide_nonuniformcube( const K3DVector* triangle, const K3DVector *cube)
|
|
{
|
|
int i;
|
|
K3DVector cv, ov;
|
|
|
|
// Triangle -> Cube
|
|
// top bottom
|
|
vec_cross_vec(cv, cube[0] - cube[1], cube[5] - cube[0]);
|
|
for(i = 0; i < 3; i++) {
|
|
ov = triangle[i] - cube[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto TRI_CUBE_TOP;
|
|
}
|
|
return false;
|
|
TRI_CUBE_TOP:
|
|
|
|
vec_cross_vec(cv, cube[2] - cube[3], cube[7] - cube[2]);
|
|
for(i = 0; i < 3; i++) {
|
|
ov = triangle[i] - cube[3];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto TRI_CUBE_BOTTOM;
|
|
}
|
|
return false;
|
|
TRI_CUBE_BOTTOM:
|
|
|
|
// left right
|
|
vec_cross_vec(cv, cube[0] - cube[5], cube[3] - cube[0]);
|
|
for(i = 0; i < 3; i++) {
|
|
ov = triangle[i] - cube[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto TRI_CUBE_LEFT;
|
|
}
|
|
return false;
|
|
TRI_CUBE_LEFT:
|
|
|
|
vec_cross_vec(cv, cube[1] - cube[2], cube[4] - cube[1]);
|
|
for(i = 0; i < 3; i++) {
|
|
ov = triangle[i] - cube[1];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto TRI_CUBE_RIGHT;
|
|
}
|
|
return false;
|
|
TRI_CUBE_RIGHT:
|
|
|
|
// front back
|
|
vec_cross_vec(cv, cube[0] - cube[3], cube[1] - cube[0]);
|
|
for(i = 0; i < 3; i++) {
|
|
ov = triangle[i] - cube[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto TRI_CUBE_FRONT;
|
|
}
|
|
return false;
|
|
TRI_CUBE_FRONT:
|
|
|
|
vec_cross_vec(cv, cube[5] - cube[4], cube[6] - cube[5]);
|
|
for(i = 0; i < 3; i++) {
|
|
ov = triangle[i] - cube[5];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto TRI_CUBE_BACK;
|
|
}
|
|
return false;
|
|
TRI_CUBE_BACK:
|
|
|
|
|
|
// Cube -> Triangle
|
|
K3DVector cp, fcp, v1, v2;
|
|
float d; // cp X (vertex) - d = 0
|
|
|
|
// Front
|
|
v1 = triangle[1] - triangle[0];
|
|
v2 = triangle[2] - triangle[1];
|
|
vec_cross_vec(cp, v1, v2);
|
|
d = vec_dot_vec(cp, triangle[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_FRONT_CUBE;
|
|
}
|
|
return false;
|
|
TRI_FRONT_CUBE:
|
|
|
|
fcp = cp; // means : front side cross product
|
|
|
|
// Back
|
|
cp.x = -cp.x; cp.y = -cp.y; cp.z = -cp.z;
|
|
d = vec_dot_vec(cp, triangle[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_BACK_CUBE;
|
|
}
|
|
return false;
|
|
TRI_BACK_CUBE:
|
|
|
|
// Edge 1
|
|
v1 = triangle[1] - triangle[0];
|
|
vec_cross_vec(cp, v1, fcp);
|
|
d = vec_dot_vec(cp, triangle[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_EDGE1_CUBE;
|
|
}
|
|
return false;
|
|
TRI_EDGE1_CUBE:
|
|
|
|
// Edge 2
|
|
v1 = triangle[2] - triangle[1];
|
|
vec_cross_vec(cp, v1, fcp);
|
|
d = vec_dot_vec(cp, triangle[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_EDGE2_CUBE;
|
|
}
|
|
return false;
|
|
TRI_EDGE2_CUBE:
|
|
|
|
// Edge 3
|
|
v1 = triangle[3] - triangle[2];
|
|
vec_cross_vec(cp, v1, fcp);
|
|
d = vec_dot_vec(cp, triangle[2]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) <= d) goto TRI_EDGE3_CUBE;
|
|
}
|
|
return false;
|
|
TRI_EDGE3_CUBE:
|
|
|
|
return true;
|
|
}
|
|
|
|
// nonuniform cube vs quad
|
|
static inline bool imp_quad_collide_nonuniformcube( const K3DVector* quad, const K3DVector *cube)
|
|
{
|
|
int i;
|
|
K3DVector cv, ov;
|
|
|
|
// Quad -> Cube
|
|
// top bottom
|
|
vec_cross_vec(cv, cube[0] - cube[1], cube[5] - cube[0]);
|
|
for(i = 0; i < 4; i++) {
|
|
ov = quad[i] - cube[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto QUAD_CUBE_TOP;
|
|
}
|
|
return false;
|
|
QUAD_CUBE_TOP:
|
|
|
|
vec_cross_vec(cv, cube[2] - cube[3], cube[7] - cube[2]);
|
|
for(i = 0; i < 4; i++) {
|
|
ov = quad[i] - cube[3];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto QUAD_CUBE_BOTTOM;
|
|
}
|
|
return false;
|
|
QUAD_CUBE_BOTTOM:
|
|
|
|
// left right
|
|
vec_cross_vec(cv, cube[0] - cube[5], cube[3] - cube[0]);
|
|
for(i = 0; i < 4; i++) {
|
|
ov = quad[i] - cube[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto QUAD_CUBE_LEFT;
|
|
}
|
|
return false;
|
|
QUAD_CUBE_LEFT:
|
|
|
|
vec_cross_vec(cv, cube[1] - cube[2], cube[4] - cube[1]);
|
|
for(i = 0; i < 4; i++) {
|
|
ov = quad[i] - cube[1];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto QUAD_CUBE_RIGHT;
|
|
}
|
|
return false;
|
|
QUAD_CUBE_RIGHT:
|
|
|
|
// front back
|
|
vec_cross_vec(cv, cube[0] - cube[3], cube[1] - cube[0]);
|
|
for(i = 0; i < 4; i++) {
|
|
ov = quad[i] - cube[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto QUAD_CUBE_FRONT;
|
|
}
|
|
return false;
|
|
QUAD_CUBE_FRONT:
|
|
|
|
vec_cross_vec(cv, cube[5] - cube[4], cube[6] - cube[5]);
|
|
for(i = 0; i < 4; i++) {
|
|
ov = quad[i] - cube[5];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto QUAD_CUBE_BACK;
|
|
}
|
|
return false;
|
|
QUAD_CUBE_BACK:
|
|
|
|
|
|
// Cube -> Quad
|
|
K3DVector cp, fcp, v1, v2;
|
|
float d; // cp X (vertex) - d = 0
|
|
|
|
// Front
|
|
vec_cross_vec(cp, quad[1] - quad[0], quad[2] - quad[1]);
|
|
d = vec_dot_vec(cp, quad[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) < d) goto QUAD_FRONT_CUBE;
|
|
}
|
|
return false;
|
|
QUAD_FRONT_CUBE:
|
|
|
|
fcp = cp; // means : front side cross product
|
|
|
|
// Back
|
|
cp.x = -cp.x; cp.y = -cp.y; cp.z = -cp.z;
|
|
d = vec_dot_vec(cp, quad[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) < d) goto QUAD_BACK_CUBE;
|
|
}
|
|
return false;
|
|
QUAD_BACK_CUBE:
|
|
|
|
// Edge 1
|
|
vec_cross_vec(cp, quad[1] - quad[0], fcp);
|
|
d = vec_dot_vec(cp, quad[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) < d) goto QUAD_EDGE1_CUBE;
|
|
}
|
|
return false;
|
|
QUAD_EDGE1_CUBE:
|
|
|
|
// Edge 2
|
|
vec_cross_vec(cp, quad[2] - quad[1], fcp);
|
|
d = vec_dot_vec(cp, quad[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) < d) goto QUAD_EDGE2_CUBE;
|
|
}
|
|
return false;
|
|
QUAD_EDGE2_CUBE:
|
|
|
|
// Edge 3
|
|
vec_cross_vec(cp, quad[3] - quad[2], fcp);
|
|
d = vec_dot_vec(cp, quad[2]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) < d) goto QUAD_EDGE3_CUBE;
|
|
}
|
|
return false;
|
|
QUAD_EDGE3_CUBE:
|
|
|
|
// Edge 4
|
|
vec_cross_vec(cp, quad[0] - quad[3], fcp);
|
|
d = vec_dot_vec(cp, quad[3]);
|
|
for(i = 0; i < 8; i++) {
|
|
if(vec_dot_vec(cp, cube[i]) < d) goto QUAD_EDGE4_CUBE;
|
|
}
|
|
return false;
|
|
QUAD_EDGE4_CUBE:
|
|
|
|
return true;
|
|
}
|
|
|
|
// nonuniform cube vs nonuniform cube
|
|
static inline bool imp_nonuniformcube_collide_nonuniformcube( const K3DVector* cube1, const K3DVector* cube2 )
|
|
{
|
|
int i;
|
|
K3DVector cv/*, cv2*/, ov;
|
|
|
|
// Cube1 -> Cube2
|
|
// top bottom
|
|
vec_cross_vec(cv, cube1[0] - cube1[1], cube1[5] - cube1[0]);
|
|
//make_vector(cv2, cube1[3], cube1[0]);
|
|
//assert(cv.x*cv2.x + cv.y*cv2.y + cv.z*cv2.z > 0);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube2[i] - cube1[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE1_CUBE2_TOP;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_TOP:
|
|
|
|
vec_cross_vec(cv, cube1[2] - cube1[3], cube1[7] - cube1[2]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube2[i] - cube1[2];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE1_CUBE2_BOTTOM;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_BOTTOM:
|
|
|
|
// left right
|
|
vec_cross_vec(cv, cube1[0] - cube1[5], cube1[3] - cube1[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube2[i] - cube1[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE1_CUBE2_LEFT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_LEFT:
|
|
|
|
vec_cross_vec(cv, cube1[1] - cube1[2], cube1[4] - cube1[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube2[i] - cube1[1];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE1_CUBE2_RIGHT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_RIGHT:
|
|
|
|
// front back
|
|
vec_cross_vec(cv, cube1[0] - cube1[3], cube1[1] - cube1[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube2[i] - cube1[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE1_CUBE2_FRONT;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_FRONT:
|
|
|
|
vec_cross_vec(cv, cube1[5] - cube1[4], cube1[6] - cube1[5]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube2[i] - cube1[5];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE1_CUBE2_BACK;
|
|
}
|
|
return false;
|
|
CUBE1_CUBE2_BACK:
|
|
|
|
|
|
// Cube2 -> Cube1
|
|
// top bottom
|
|
vec_cross_vec(cv, cube2[0] - cube2[1], cube2[5] - cube2[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube1[i] - cube2[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE2_CUBE1_TOP;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_TOP:
|
|
|
|
vec_cross_vec(cv, cube2[2] - cube2[3], cube2[7] - cube2[2]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube1[i] - cube2[2];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE2_CUBE1_BOTTOM;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_BOTTOM:
|
|
|
|
// left right
|
|
vec_cross_vec(cv, cube2[0] - cube2[5], cube2[3] - cube2[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube1[i] - cube2[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE2_CUBE1_LEFT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_LEFT:
|
|
|
|
vec_cross_vec(cv, cube2[1] - cube2[2], cube2[4] - cube2[1]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube1[i] - cube2[1];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE2_CUBE1_RIGHT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_RIGHT:
|
|
|
|
// front back
|
|
vec_cross_vec(cv, cube2[0] - cube2[3], cube2[1] - cube2[0]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube1[i] - cube2[0];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE2_CUBE1_FRONT;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_FRONT:
|
|
|
|
vec_cross_vec(cv, cube2[5] - cube2[4], cube2[6] - cube2[5]);
|
|
for(i = 0; i < 8; i++) {
|
|
ov = cube1[i] - cube2[5];
|
|
if(vec_dot_vec(cv, ov) >= 0) goto CUBE2_CUBE1_BACK;
|
|
}
|
|
return false;
|
|
CUBE2_CUBE1_BACK:
|
|
|
|
return true;
|
|
}
|
|
|
|
static inline bool imp_point_collide_cylinder( const K3DVector* point, const K3DVector* APole, const K3DVector* BPole, float radius )
|
|
{
|
|
if(radius <= 0) return false;
|
|
|
|
// pole의 벡터를 만든다.
|
|
K3DVector ppole = *APole - *BPole;
|
|
K3DVector npole = -ppole;
|
|
K3DVector ptoAPole = *point - *APole;
|
|
K3DVector ptoBPole = *point - *BPole;
|
|
|
|
// A Plane에 대해 벗어나는지 체크
|
|
if(vec_dot_vec(ppole, npole)*vec_dot_vec(ppole, ptoAPole) < 0)
|
|
return false;
|
|
// B Plane에 대해 벗어나는지 체크
|
|
if(vec_dot_vec(ppole, ppole)*vec_dot_vec(ppole, ptoBPole) < 0)
|
|
return false;
|
|
// pole에 대한 거리를 계산한다. : 헤깔 - B-A인지 A-B인지...
|
|
float ptoAPolelen = vec_distance(ptoAPole);
|
|
float pPolelen = vec_distance(ppole);
|
|
if(ptoAPolelen*pPolelen <= 0)
|
|
return false;
|
|
float cost = vec_dot_vec(ptoAPole, ppole)
|
|
/(ptoAPolelen*pPolelen);
|
|
if(cost > 1)
|
|
return false;
|
|
float res = ptoAPolelen * (float)sqrt(1 - cost*cost);
|
|
if(res > radius)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
};
|
|
|
|
|
|
typedef class PrimitiveCollisionCheckPackage KPCCP;
|
|
|
|
#endif
|