Sunday, August 3, 2014

OBB-OBB Collision

I. Important variables:

struct tOBB{
tR4x4 m;/*matrix = XYZ axes (A unit vector) + position V*/
tR03 e;/*Half-widths from center, at XYZ*/};

T = B.m.V – A.m.V

M = A 3x3 matrix
/ dot(A.m.X,B.m.X) dot(A.m.X,B.m.Y) dot(A.m.X,B.m.Z) \
| dot(A.m.Y,B.m.X) dot(A.m.Y,B.m.Y) dot(A.m.Y,B.m.Z) |
\ dot(A.m.Z,B.m.X) dot(A.m.Z,B.m.Y) dot(A.m.Z,B.m.Z) /
N = abs(M) (absolute values of all 9 members)

IN = Index
L = Separating Axis (3D vector)
Ra = Projected Radius A (scalar)
Rb = Projected Radius B (scalar)
Rab = Ra+Rb
T.L = Dot Product of T and L (scalar)
|T.L| = The absolute value (of T.L)
D = Delta (depth of intersection) (scalar)
DN = Delta, normalized (depth of intersection, relative to projected radius)

First 6 rows = to determine whether any vertices of the two OBB's are penetrating any faces
Last 9 rows = to determine whether any edges of the two OBB's are intersecting each other

 (TODO: Table without transformation)

(For efficiency, Volume B's coordinates are transformed to Volume A's space, as it is summarized in this table below)

INL (not transformed, original)Ra (transformed)Rb (transformed)T.L (transformed)
0A.m.XA.e.xB.e.x*N.N11+
B.e.y*N.N12+
B.e.z*N.N13		T.x
1A.m.YA.e.yB.e.x*N.N21+
B.e.y*N.N22+
B.e.z*N.N23		T.y
2A.m.ZA.e.zB.e.x*N.N31+
B.e.y*N.N32+
B.e.z*N.N33		T.z
3B.m.Xdot(A.e,N.X)B.e.xdot(T,M.X)
4B.m.Ydot(A.e,N.Y)B.e.ydot(T,M.Y)
5B.m.Zdot(A.e,N.Z)B.e.zdot(T,M.Z)
6cross(A.m.X,B.m.X)A.e.y*N.N31+
A.e.z*N.N21		B.e.y*N.N13+
B.e.z*N.N12		T.z*M.N21-T.y*M.N31
7cross(A.m.X,B.m.Y)A.e.y*N.N32+
A.e.z*N.N22		B.e.x*N.N13+
B.e.z*N.N11		T.z*M.N22-T.y*M.N32
8cross(A.m.X,B.m.Z)A.e.y*N.N33+
A.e.z*N.N23		B.e.x*N.N12+
B.e.y*N.N11		T.z*M.N23-T.y*M.N33
9cross(A.m.Y,B.m.X)A.e.x*N.N31+
A.e.z*N.N11		B.e.y*N.N23+
B.e.z*N.N22		T.x*M.N31-T.z*M.N11
Across(A.m.Y,B.m.Y)A.e.x*N.N32+
A.e.z*N.N12		B.e.x*N.N23+
B.e.z*N.N21		T.x*M.N32-T.z*M.N12
Bcross(A.m.Y,B.m.Z)A.e.x*N.N33+
A.e.z*N.N13		B.e.x*N.N22+
B.e.y*N.N21		T.x*M.N33-T.z*M.N13
Ccross(A.m.Z,B.m.X)A.e.x*N.N21+
A.e.y*N.N11		B.e.y*N.N33+
B.e.z*N.N32		T.y*M.N11-T.x*M.N21
Dcross(A.m.Z,B.m.Y)A.e.x*N.N22+
A.e.y*N.N12		B.e.x*N.N33+
B.e.z*N.N31		T.y*M.N12-T.x*M.N22
Ecross(A.m.Z,B.m.Z)A.e.x*N.N23+
A.e.y*N.N13		B.e.x*N.N32+
B.e.y*N.N31		T.y*M.N13-T.x*M.N23

For response:
Sign = Whether T.L is non-negative (zero or positive) (value: 0) or negative (value: 1)
T = Correction vector (for collision response)


INSignTINSignT (simply a negation)
00A.e.x01-A.e.x
10A.e.y11-A.e.y
20A.e.z21-A.e.z
30B.e.X31-B.e.X
40B.e.Y41-B.e.Y
50B.e.Z51-B.e.Z
60cross(A.m.X,B.m.X)61-cross(A.m.X,B.m.X)
70cross(A.m.X,B.m.Y)71-cross(A.m.X,B.m.Y)
80cross(A.m.X,B.m.Z)81-cross(A.m.X,B.m.Z)
90cross(A.m.Y,B.m.X)91-cross(A.m.Y,B.m.X)
A0cross(A.m.Y,B.m.Y)A1-cross(A.m.Y,B.m.Y)
B0cross(A.m.Y,B.m.Z)B1-cross(A.m.Y,B.m.Z)
C0cross(A.m.Z,B.m.X)C1-cross(A.m.Z,B.m.X)
D0cross(A.m.Z,B.m.Y)D1-cross(A.m.Z,B.m.Y)
E0cross(A.m.Z,B.m.Z)E1-cross(A.m.Z,B.m.Z)

*
http://qaop.co.uk/docs/OBBOBBIntersect.pdf
http://realtimecollisiondetection.net/

II. Algorithm:

Compute  Rab  and  |T.L|, compute D/DN/IN/SIGN.

While going through 15 cases, please track which case has the smallest DN. Save the values D, IN, and the sign of T.L (whether T.L is negative or positive), from the case which has the smallest DN.
Save these values while you are tracking DN.

You will need to use: D to displace your OBB volumes; and, IN and the sign of T.L to determine in which direction to do the displacement.


In the first case:
Compute Rab, compute T.L.
if Rab<=|T.L| == true
    There is no collision. Return "no collision".
else
    Save D.     D   = Rab - |T.L|
    Save DN.  DN = D / Rab
    Save SIGN = TL < 0.
    Save IN = 0.
    Proceed to next case.
endif

In the next 5 cases:
Compute Rab, compute T.L.
if Rab<=|T.L| == true
    There is no collision. Return "no collision".
else
    ("temp" - temporary)
    Compute D-temp.     D-temp   = Rab - |T.L|
    Compute DN-temp.  DN-temp = D-temp / Rab
    if   DN-temp < DN
        Update D.      D    =  D-temp
        Update DN.   DN =  DN-temp
        Save (and therefore update) SIGN = TL < 0.
        Save IN = current case (between inclusive 1 and inclusive 5).
    else, do nothing.
    endif
    Proceed to next case.
endif

In the last 9 cases:
Compute Rab, compute T.L.
if Rab<=|T.L| == true
    if Rab != null
        There is no collision. Return "no collision".
    else
        Proceed to next case.
    endif
else
    Compute D-temp.     D-temp   = Rab - |T.L|
    Compute DN-temp.  DN-temp = D-temp / Rab
    if   DN-temp < DN
        Update D.      D    =  D-temp
        Update DN.   DN =  DN-temp
        Save (and therefore update) SIGN = TL < 0.
        Save IN = current case (between inclusive 6 and inclusive 14).
    else, do nothing.
    endif
    Proceed to next case.
endif

WARNING: In the last 9 cases, The value of   Rab  can potentially be zero, when  L  becomes the cross product of perpendicular vectors. (That is, when the orientation of the two OBB volumes become aligned and the OBB volumes behave like AABB volumes)
For example, this would happen if:
A.m.X==B.m.X&&A.m.Y==B.m.Y&&A.m.Z==B.m.Z, or
A.m.X==B.m.Y&&A.m.Y==B.m.Z&&A.m.Z==B.m.A, etc.
This is why you must use the "smaller-equal than" operator (Rab<=|T.L| == true). If the "smaller than" operator has been used instead (Rab<|T.L| == true), and when both   Rab  and  |T.L|   ever become zero, there will be a division by zero (when DN is being computed, because you need to do division by  Rab  ).

(UPDATE: 2015-01-28-We)

To repeat, if a vector of both OBB-volumes become aligned (For example, both OBB-volumes have straight-up Y-axes, both volumes are rotated only about the worldspace's Y-axis), then Rab can ever be zero. Skip such a case and proceed to next case.



After all 15 cases, then it is determined: "There is collision!"

To create a collision response:
T  = [ Check the table above ]
T *=  D * 0.5

A -= T;
B += T;


III. Video:


https://www.youtube.com/watch?v=YeZWknkneuM

IV. Code:
/*Bloody C99 keywords, I deny!*/
#include<stdint.h>
typedef uint8_t t1u;
typedef int8_t t1s;
typedef uint16_t t2u;
typedef int16_t t2s;
typedef uint32_t t4u;
typedef int32_t t4s;typedef float t4f;
typedef uint64_t t8u;
typedef int64_t t8s;typedef double t8f;

/*Haters gonna hate ... */
typedef void v;
#define ig else if
#define ih else
#define rK noexcept
#define rU const noexcept
#define r__e break;case
#include<cmath>

/*Header -- Dummy header -- Please supply your own implementation!*/
typedef float tR;
struct tR03{tR m[3];
tR&A()rK{return m[0];}tR const&A()rU{return m[0];}
tR&B()rK{return m[1];}tR const&B()rU{return m[1];}
tR&C()rK{return m[2];}tR const&C()rU{return m[2];}
v operator+=(tR03 const&)rK;
v operator-=(tR03 const&)rK;
tR03 operator+(tR03 const&)rU;
tR03 operator-(tR03 const&)rU;
tR03 operator*(tR const&)rU;
tR03 operator/(tR const&)rU;
tR kD(tR03 const&)rU;/*Dot-product*/};
tR03 cross(tR03 const&,tR03 const&);

#define o_(a,b)\
tR const&N##a##b()rU{return m[((b-1)<<2)+(a-1)];}\
tR&N##a##b()rK{return m[((b-1)<<2)+(a-1)];}
struct tR3x3{tR m[9];
o_(1,1)o_(1,2)o_(1,3)
o_(2,1)o_(2,2)o_(2,3)
o_(3,1)o_(3,2)o_(3,3)
tR03&X()rK{return(tR03&)m[0x00];}tR03 const&X()rU{return(tR03 const&)m[0x00];}
tR03&Y()rK{return(tR03&)m[0x03];}tR03 const&Y()rU{return(tR03 const&)m[0x03];}
tR03&Z()rK{return(tR03&)m[0x06];}tR03 const&Z()rU{return(tR03 const&)m[0x06];}};
struct tR4x4{tR m[16];
o_(1,1)o_(1,2)o_(1,3)o_(1,4)
o_(2,1)o_(2,2)o_(2,3)o_(2,4)
o_(3,1)o_(3,2)o_(3,3)o_(3,4)
o_(4,1)o_(4,2)o_(4,3)o_(4,4)
tR03&X()rK{return(tR03&)m[0x00];}tR03 const&X()rU{return(tR03 const&)m[0x00];}
tR03&Y()rK{return(tR03&)m[0x04];}tR03 const&Y()rU{return(tR03 const&)m[0x04];}
tR03&Z()rK{return(tR03&)m[0x08];}tR03 const&Z()rU{return(tR03 const&)m[0x08];}
tR03&V()rK{return(tR03&)m[0x0C];}tR03 const&V()rU{return(tR03 const&)m[0x0C];}};
#undef o_

struct tOBB{
tR4x4 m;
tR03 e;};

typedef tR tRC;/*Scalar, displacement*/
typedef tR03 tR03C;/*Vector, displacement; You be the judge, whether you should differentiate the types*/


/*Source*/

#define o_OBTL1(p,q,tRC)TLA=std::abs(TL=(p));if(Rab<=TLA){return 0;}DN=(D=Rab-TLA)/Rab;i=TL<tRC(0)?0x10:0;
#define o_OBTLN(p,q,tRC)TLA=std::abs(TL=(p));if(Rab<=TLA){return 0;}DTN=(DT=Rab-TLA)/Rab;if(DTN<DN){DN=DTN;D=DT;i=(q)|(TL<tRC(0)?0x10:0);}

#define o_OBTLO(p,q,tRC)TLA=std::abs(TL=(p));if(Rab<=TLA){if(Rab!=0){return 0;}}ih{DTN=(DT=Rab-TLA)/Rab;if(DTN<DN){DN=DTN;D=DT;i=(q)|(TL<tRC(0)?0x10:0);}}
/*#define o_OB_V2(p,q,r,tRC)Rab=G.kD(N.p())+H.q();o_OBTLN(T.kD(M.p()),r,tRC)
#define o_OB_Ej(a,b,c,d,e,f,g,h,i,j,tRC)\
Rab=G.a()*N.N##b##1()+G.c()*N.N##d##1()+H.B()*N.N##e##3()+H.C()*N.N##e##2();o_OBTLO(T.f()*M.N##g##1()-T.h()*M.N##i##1(),6+j,tRC)\
Rab=G.a()*N.N##b##2()+G.c()*N.N##d##2()+H.A()*N.N##e##3()+H.C()*N.N##e##1();o_OBTLO(T.f()*M.N##g##2()-T.h()*M.N##i##2(),7+j,tRC)\
Rab=G.a()*N.N##b##3()+G.c()*N.N##d##3()+H.A()*N.N##e##2()+H.B()*N.N##e##1();o_OBTLO(T.f()*M.N##g##3()-T.h()*M.N##i##3(),8+j,tRC)*/

t4u fDOBOB3(tOBB&p,tOBB&q)rK{
tR03C&A=(tR03C&)p.m.V(),&B=(tR03C&)q.m.V();
tR03C const&G=(tR03C&)p.e,&H=(tR03C&)q.e;
tR03C const BA(B-A);tR03C T;tR3x3 M,N;
tRC Rab,TLA,TL,D,DT;tR DN,DTN;t4u i;
/*IN:0*/
T.A()=BA.kD(p.m.X()),
N.N11()=std::abs(M.N11()=p.m.X().kD(q.m.X())),
N.N12()=std::abs(M.N12()=p.m.X().kD(q.m.Y())),
N.N13()=std::abs(M.N13()=p.m.X().kD(q.m.Z()));/*Initializes matrices M and N, late*/
Rab=G.A()+(H.A()*N.N11()+H.B()*N.N12()+H.C()*N.N13());
o_OBTL1(T.A(),0x0,tRC)
/*IN:1*/
T.B()=BA.kD(p.m.Y()),
N.N21()=std::abs(M.N21()=p.m.Y().kD(q.m.X())),
N.N22()=std::abs(M.N22()=p.m.Y().kD(q.m.Y())),
N.N23()=std::abs(M.N23()=p.m.Y().kD(q.m.Z()));/*It is late, to save process from unnecessarily calculating values, ...*/
Rab=G.B()+(H.A()*N.N21()+H.B()*N.N22()+H.C()*N.N23());
o_OBTLN(T.B(),0x1,tRC)
/*IN:2*/
T.C()=BA.kD(p.m.Z()),
N.N31()=std::abs(M.N31()=p.m.Z().kD(q.m.X())),
N.N32()=std::abs(M.N32()=p.m.Z().kD(q.m.Y())),
N.N33()=std::abs(M.N33()=p.m.Z().kD(q.m.Z()));/*... in case function terminates early*/
Rab=G.C()+(H.A()*N.N31()+H.B()*N.N32()+H.C()*N.N33());
o_OBTLN(T.C(),0x2,tRC)
/*IN:3 o_OB_V2(X,A,3,tRC)*/
Rab=G.kD(N.X())+H.A();
o_OBTLN(T.kD(M.X()),0x3,tRC)
/*IN:4 o_OB_V2(Y,B,4,tRC)*/
Rab=G.kD(N.Y())+H.B();
o_OBTLN(T.kD(M.Y()),0x4,tRC)
/*IN:5 o_OB_V2(Z,C,5,tRC)*/
Rab=G.kD(N.Z())+H.C();
o_OBTLN(T.kD(M.Z()),0x5,tRC)
/*IN:6*//*IN:6 7 8 o_OB_Ej(B,3,C,2,1,C,2,B,3,0,tRC)*/
Rab=G.B()*N.N31()+G.C()*N.N21()+H.B()*N.N13()+H.C()*N.N12();
o_OBTLO(T.C()*M.N21()-T.B()*M.N31(),0x6,tRC)
/*IN:7*/
Rab=G.B()*N.N32()+G.C()*N.N22()+H.A()*N.N13()+H.C()*N.N11();
o_OBTLO(T.C()*M.N22()-T.B()*M.N32(),0x7,tRC)
/*IN:8*/
Rab=G.B()*N.N33()+G.C()*N.N23()+H.A()*N.N12()+H.B()*N.N11();
o_OBTLO(T.C()*M.N23()-T.B()*M.N33(),0x8,tRC)
/*IN:9*//*IN:9 A B o_OB_Ej(A,3,C,1,2,A,3,C,1,3,tRC)*/
Rab=G.A()*N.N31()+G.C()*N.N11()+H.B()*N.N23()+H.C()*N.N22();
o_OBTLO(T.A()*M.N31()-T.C()*M.N11(),0x9,tRC)
/*IN:A*/
Rab=G.A()*N.N32()+G.C()*N.N12()+H.A()*N.N23()+H.C()*N.N21();
o_OBTLO(T.A()*M.N32()-T.C()*M.N12(),0xA,tRC)
/*IN:B*/
Rab=G.A()*N.N33()+G.C()*N.N13()+H.A()*N.N22()+H.B()*N.N21();
o_OBTLO(T.A()*M.N33()-T.C()*M.N13(),0xB,tRC)
/*IN:C*//*IN:C D E o_OB_Ej(A,2,B,1,3,B,1,A,2,6,tRC)*/
Rab=G.A()*N.N21()+G.B()*N.N11()+H.B()*N.N33()+H.C()*N.N32();
o_OBTLO(T.B()*M.N11()-T.A()*M.N21(),0xC,tRC)
/*IN:D*/
Rab=G.A()*N.N22()+G.B()*N.N12()+H.A()*N.N33()+H.C()*N.N31();
o_OBTLO(T.B()*M.N12()-T.A()*M.N22(),0xD,tRC)
/*IN:E*/
Rab=G.A()*N.N23()+G.B()*N.N13()+H.A()*N.N32()+H.B()*N.N31();
o_OBTLO(T.B()*M.N13()-T.A()*M.N23(),0xE,tRC)
switch(i){
case 0x00:T=(p.m.X())*(D*0.5F);r__e 0x01:T=(p.m.Y())*(D*0.5F);
r__e 0x02:T=(p.m.Z())*(D*0.5F);r__e 0x03:T=(q.m.X())*(D*0.5F);
r__e 0x04:T=(q.m.Y())*(D*0.5F);r__e 0x05:T=(q.m.Z())*(D*0.5F);
r__e 0x06:T=cross(p.m.X(),q.m.X())*(D*0.5F);r__e 0x07:T=cross(p.m.X(),q.m.Y())*(D*0.5F);r__e 0x08:T=cross(p.m.X(),q.m.Z())*(D*0.5F);
r__e 0x09:T=cross(p.m.Y(),q.m.X())*(D*0.5F);r__e 0x0A:T=cross(p.m.Y(),q.m.Y())*(D*0.5F);r__e 0x0B:T=cross(p.m.Y(),q.m.Z())*(D*0.5F);
r__e 0x0C:T=cross(p.m.Z(),q.m.X())*(D*0.5F);r__e 0x0D:T=cross(p.m.Z(),q.m.Y())*(D*0.5F);r__e 0x0E:T=cross(p.m.Z(),q.m.Z())*(D*0.5F);
r__e 0x10:T=p.m.X()*(D*-0.5F);r__e 0x11:T=p.m.Y()*(D*-0.5F);
r__e 0x12:T=p.m.Z()*(D*-0.5F);r__e 0x13:T=q.m.X()*(D*-0.5F);
r__e 0x14:T=q.m.Y()*(D*-0.5F);r__e 0x15:T=q.m.Z()*(D*-0.5F);
r__e 0x16:T=cross(p.m.X(),q.m.X())*(D*-0.5F);r__e 0x17:T=cross(p.m.X(),q.m.Y())*(D*-0.5F);r__e 0x18:T=cross(p.m.X(),q.m.Z())*(D*-0.5F);
r__e 0x19:T=cross(p.m.Y(),q.m.X())*(D*-0.5F);r__e 0x1A:T=cross(p.m.Y(),q.m.Y())*(D*-0.5F);r__e 0x1B:T=cross(p.m.Y(),q.m.Z())*(D*-0.5F);
r__e 0x1C:T=cross(p.m.Z(),q.m.X())*(D*-0.5F);r__e 0x1D:T=cross(p.m.Z(),q.m.Y())*(D*-0.5F);r__e 0x1E:T=cross(p.m.Z(),q.m.Z())*(D*-0.5F);break;default:return 0x00;break;}
A-=T;B+=T;return 1;}
Posted by at

2 comments:

  1. UnknownDecember 16, 2014 at 5:06 AM

    Hi, your implementation is very good.
    Could you post more readable C++ code?
    There are too many defines and symbols, beginners like me cannot understand it easily. :(
    Thank you. :)

    ReplyDelete
  2. AnonymousApril 8, 2015 at 8:16 AM

    Don't bother using the code given, unless you want to get ideas for your next IOCCC submission.

    ReplyDelete

Subscribe to: Post Comments (Atom)