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;}

Saturday, August 2, 2014

Fixed-point square-root

Simple implementation ...
Continuing from the previously posted fixed-point type ... , (UPDATE: My apology, because there was an error ... -- It's been fixed .. > <)

Output (reformatted, partial):
tRX4::sqrt(0.0000)=    0.0000   < 0          >   0          =sqrt(0)
tRX4::sqrt(0.1250)=    0.3515   <-0.00199088 >   0.353553   =sqrt(0.125)
tRX4::sqrt(0.2500)=    0.5000   < 0          >   0.5        =sqrt(0.25)
tRX4::sqrt(0.3750)=    0.6093   <-0.00299746 >   0.612372   =sqrt(0.375)
tRX4::sqrt(0.5000)=    0.7070   <-7.55191e-05>   0.707107   =sqrt(0.5)
tRX4::sqrt(0.6250)=    0.7890   <-0.00150692 >   0.790569   =sqrt(0.625)
tRX4::sqrt(0.7500)=    0.8632   <-0.00274414 >   0.866025   =sqrt(0.75)
tRX4::sqrt(0.8750)=    0.9335   <-0.00182062 >   0.935414   =sqrt(0.875)
tRX4::sqrt(1.0000)=    1.0000   < 0          >   1          =sqrt(1)
tRX4::sqrt(1.1250)=    1.0585   <-0.00206637 >   1.06066    =sqrt(1.125)
...

tRX4::sqrt(2.0000)=    1.4140   <-0.000151038>   1.41421    =sqrt(2)
tRX4::sqrt(2.1250)=    1.4570   <-0.000706673>   1.45774    =sqrt(2.125)
tRX4::sqrt(2.2500)=    1.5000   < 0          >   1.5        =sqrt(2.25)
tRX4::sqrt(2.3750)=    1.5390   <-0.00204098 >   1.5411     =sqrt(2.375)
...

tRX4::sqrt(3.8750)=    1.9648   <-0.00365818 >   1.9685     =sqrt(3.875)
tRX4::sqrt(4.0000)=    2.0000   < 0          >   2          =sqrt(4)
tRX4::sqrt(4.1250)=    2.0273   <-0.00366592 >   2.03101    =sqrt(4.125)
...

tRX4::sqrt(6.1250)=    2.4726   <-0.00221753 >   2.47487    =sqrt(6.125)
tRX4::sqrt(6.2500)=    2.5000   < 0          >   2.5        =sqrt(6.25)
tRX4::sqrt(6.3750)=    2.5234   <-0.00143862 >   2.52488    =sqrt(6.375)
...

tRX4::sqrt(8.0000)=    2.8281   <-0.000302076>   2.82843    =sqrt(8)

Code:
typedef uint32_t t4u;
typedef int32_t t4s;
typedef float t4f;

struct tRX4;/*The fixed-point type is a struct; Refer to the previous tutorial*/
typedef t4u tWU4;/*whole number, unsigned, 4 bytes*/
typedef t4s tII4;/*Integer, signed, 4 bytes*/

/*Would it ever be approriate, to implement it as a struct?
struct tWU4;
struct tII4;*/


static t4u wsLog2(t4u p)noexcept{static t4u const n[0x20]={
0x00,0x09,0x01,0x0A,0x0D,0x15,0x02,0x1D,0x0B,0x0E,0x10,0x12,0x16,0x19,0x03,0x1E,
0x08,0x0C,0x14,0x1C,0x0F,0x11,0x18,0x07,0x13,0x1B,0x17,0x06,0x1A,0x05,0x04,0x1F};
p|=p>>0x01,p|=p>>0x02,p|=p>>0x04,p|=p>>0x08,p|=p>>0x10;return n[(p*0x07C4ACDDU)>>0x1B];}

static tRX4 sqrt(tRX4 const&p)noexcept{
    tRX4 l=(tRX4)tWU4(1<<(wsLog2(t4u(p.g())>>tRX4::cFc)>>1));
    /*With mantissa of 8 bits (see previous tutorial page for fixed-point type),
    according to tests, the maximum loop-sum is 9.
    If you can guarantee that the parameter will never be zero,
    then you can slightly increase this loop-sum value for accuracy*/
    t4u i=0x09;

    while(i--){
        l=(p/l+l)/tWU4(2);}

    return l;}

#include<cmath>

static t4f fsf4(tRX4 const&p)noexcept{return(t4f)p.g()/(t4f)tRX4::c1.g();}

static v f()noexcept{
tRX4 l1(0),l2;t4f m1=0.F,m2;

for(;m1<=8.F;m1+=0.125F,l1+=tRX4(0.125F)){

    l2=sqrt(l1),m2=std::sqrt(m1);
    std::cout<<"tRX4::sqrt("<<l1<< ")=    "<<l2<<"   <" 

    <<(fsf4(l2)-m2)<<">   "<<m2<<"    =sqrt("<<m1<<")"<<std::endl;}}

int main(){f();getchar();return 0;}

Bitcount, 8-bit precomputed

/*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;

static t1u const nlA[0x100]={
0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8};


static t1u fs(t1u p)noexcept{

return nlA[p];}
static t2u fs(t2u p)noexcept{

return nlA[p&0xFF]+nlA[(p>>0x08)&0xFF];}
static t4u fs(t4u p)noexcept{

return nlA[p&0xFF]+nlA[(p>>0x08)&0xFF]+nlA[(p>>0x10)&0xFF]+nlA[(p>>0x18)&0xFF];}
static t8u fs(t8u p)noexcept{return nlA[p&0xFF]+nlA[(p>>0x08)&0xFF]+nlA[(p>>0x10)&0xFF]+nlA[(p>>0x18)&0xFF]+

nlA[(p>>0x20)&0xFF]+nlA[(p>>0x28)&0xFF]+nlA[(p>>0x30)&0xFF]+nlA[(p>>0x38)&0xFF];}

Friday, August 1, 2014

Cross product of 4D vectors

| i  j  k  l  |   | ay*(bz*cw - cz*bw) - az*(by*cw - cy*bw) + aw*(by*cz - cy*bz) |
| ax ay az aw |   |-ax*(bz*cw - cz*bw) + az*(bx*cw - cx*bw) - aw*(bx*cz - cx*bz) |
| bx by bz bw | = | ax*(by*cw - cy*bw) - ay*(bx*cw - cx*bw) + aw*(bx*cy - cx*by) |
| cx cy cz cw |   |-ax*(by*cz - cy*bz) + ay*(bx*cz - cx*bz) - az*(bx*cy - cx*by) |



/*Your real-number data-type*/
/*typedef float tR;*/
/*typedef tRX4 tR;*/    /*From my previous post on fixed-point data-type*/

/*tR04 is a 4D vector (of ABCD axes); Implementation is trivial*/

static tR04 xMlX(tR04 const&a,tR04 const&b,tR04 const&c)rK{
tR
lbxcy(b.A()*c.B()),lcxby(c.A()*b.B()),lbycz(b.B()*c.C()),lcybz(c.B()*b.C()),
lbzcx(b.C()*c.A()),lczbx(c.C()*b.A()),lbzcw(b.C()*c.D()),lczbw(c.C()*b.D()),
lbwcx(b.D()*c.A()),lcwbx(c.D()*b.A()),lbwcy(b.D()*c.B()),lcwby(c.D()*b.B());
tR
lbxcyNcxby(lbxcy-lcxby),lbyczNcybz(lbycz-lcybz),lbzcxNczbx(lbzcx-lczbx),
lbzcwNczbw(lbzcw-lczbw),lbwcxNcwbx(lbwcx-lcwbx),lbwcyNcwby(lbwcy-lcwby);
return tR04(
a.B()*lbzcwNczbw+a.C()*lbwcyNcwby+a.D()*lbyczNcybz,
a.D()*lbzcxNczbx-a.A()*lbzcwNczbw-a.C()*lbwcxNcwbx,
a.D()*lbxcyNcxby-a.A()*lbwcyNcwby+a.B()*lbwcxNcwbx,
a.B()*lbzcxNczbx-a.A()*lbyczNcybz-a.C()*lbxcyNcxby);}

Wednesday, July 23, 2014

Transpose matrix with SSE2

Output:
Matrix:
| 1  2  3  4 |
| 5  6  7  8 |
| 8  7  6  5 |
| 4  3  2  1 |

Matrix, transposed:
| 1  5  8  4 |
| 2  6  7  3 |
| 3  7  6  2 |
| 4  8  5  1 |


Code:
/*Please refactor identifiers, as you like*/

#define eCG/*G++*/
/*#define eCV*//*VC++*/

#ifdef eCG
#define okN(p)__attribute__((aligned(p)))
#elif defined eCV
#define okN(p)__declspec(align(p))
#endif

#include<emmintrin.h>
#include<iostream>

struct okN(0x10) tR4x4{
float m[0x10];/*Column-major, not row-major -- OpenGL FTW*/

#define o_(a,b)\
float N##a##b()const noexcept{return m[((b-1)<<2)+(a-1)];}\
float&N##a##b()noexcept{return m[((b-1)<<2)+(a-1)];}
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)
#undef o_

tR4x4(
float p11,float p12,float p13,float p14,
float p21,float p22,float p23,float p24,
float p31,float p32,float p33,float p34,
float p41,float p42,float p43,float p44
)noexcept{
#define o_(a,b)N##a##b()=p##a##b;
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)
#undef o_
}

tR4x4(__m128i const&a,__m128i const&b,__m128i const&c,__m128i const&d)noexcept{
((__m128i*)this)[0]=a,((__m128i*)this)[1]=b,
((__m128i*)this)[2]=c,((__m128i*)this)[3]=d;}

#if 0/*Without SIMD SSE2*/
tR4x4 kTp()const noexcept{return tR4x4(
N11(),N21(),N31(),N41(),
N12(),N22(),N32(),N42(),
N13(),N23(),N33(),N43(),
N14(),N24(),N34(),N44());}
#endif

tR4x4 kTp()const noexcept{__m128i const
l1=_mm_unpacklo_epi32(((__m128i*)this)[0],((__m128i*)this)[2]),
l2=_mm_unpacklo_epi32(((__m128i*)this)[1],((__m128i*)this)[3]),
l3=_mm_unpackhi_epi32(((__m128i*)this)[0],((__m128i*)this)[2]),
l4=_mm_unpackhi_epi32(((__m128i*)this)[1],((__m128i*)this)[3]);
return tR4x4(_mm_unpacklo_epi32(l1,l2),_mm_unpackhi_epi32(l1,l2),_mm_unpacklo_epi32(l3,l4),_mm_unpackhi_epi32(l3,l4));}

friend std::ostream&operator<<(std::ostream&,tR4x4 const&)noexcept;};

std::ostream&operator<<(std::ostream&q,tR4x4 const&p)noexcept{q
<<"| "<<p.N11()<<"  "<<p.N12()<<"  "<<p.N13()<<"  "<<p.N14()<<" |\n"
<<"| "<<p.N21()<<"  "<<p.N22()<<"  "<<p.N23()<<"  "<<p.N24()<<" |\n"
<<"| "<<p.N31()<<"  "<<p.N32()<<"  "<<p.N33()<<"  "<<p.N34()<<" |\n"
<<"| "<<p.N41()<<"  "<<p.N42()<<"  "<<p.N43()<<"  "<<p.N44()<<" |\n";return q;}

void f()noexcept{
tR4x4 l(
1.F,2.F,3.F,4.F,
5.F,6.F,7.F,8.F,
8.F,7.F,6.F,5.F,
4.F,3.F,2.F,1.F);

std::cout<<"Matrix:\n"<<l<<std::endl;
std::cout<<"Matrix, transposed:\n"<<l.kTp()<<std::endl;}

int main(){f();return 0;}

Monday, July 21, 2014

Securely compare two memory blocks

This function would be necessary to prevent timing attack, when you compare two passwords or two passphrases.

#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;
typedef void v;


/*Compare two memory blocks, both of equal size 'O'
1, if same
0, if not the same*/
bool xbEqScr(v const*p,v const*q,t4u O)noexcept{t1u const*A,*B;t8u F=0;t4u l=O>>3;O&=7;
if(l){t8u const*C=(t8u const*)p,*D=(t8u const*)q;
A=O+(t1u const*)(C+l);
B=O+(t1u const*)(D+l);
while(l--){F|=*C++^*D++;}}
else{A=O+(t1u const*)p,B=O+(t1u const*)q;}
switch(O){
case 7:F|=*--A^*--B;
case 6:F|=*--A^*--B;
case 5:F|=*--A^*--B;
case 4:F|=*--A^*--B;
case 3:F|=*--A^*--B;
case 2:F|=*--A^*--B;
case 1:F|=*--A^*--B;}return!F;}

Test endianness

#include<stdint.h>
typedef uint16_t t2u;

/*1, if little; 0, if big*/
bool constexpr bEL()noexcept{return(t2u&)"AB"==(t2u)0x4241;}