Revision a4999c9d
Added by Leszek Koltunski over 1 year ago
src/main/java/org/distorted/library/helpers/QuatHelper.java | ||
---|---|---|
49 | 49 |
return new Static4D(tx,ty,tz,tw); |
50 | 50 |
} |
51 | 51 |
|
52 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
|
53 |
// return quat1*quat2 |
|
54 |
|
|
55 |
public static float[] quatMultiply( float[] quat1, float[] quat2 ) |
|
56 |
{ |
|
57 |
float qx = quat1[0]; |
|
58 |
float qy = quat1[1]; |
|
59 |
float qz = quat1[2]; |
|
60 |
float qw = quat1[3]; |
|
61 |
|
|
62 |
float rx = quat2[0]; |
|
63 |
float ry = quat2[1]; |
|
64 |
float rz = quat2[2]; |
|
65 |
float rw = quat2[3]; |
|
66 |
|
|
67 |
float tx = rw*qx - rz*qy + ry*qz + rx*qw; |
|
68 |
float ty = rw*qy + rz*qx + ry*qw - rx*qz; |
|
69 |
float tz = rw*qz + rz*qw - ry*qx + rx*qy; |
|
70 |
float tw = rw*qw - rz*qz - ry*qy - rx*qx; |
|
71 |
|
|
72 |
return new float[] {tx,ty,tz,tw}; |
|
73 |
} |
|
74 |
|
|
52 | 75 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
53 | 76 |
// return quat1*(rx,ry,rz,rw) |
54 | 77 |
|
... | ... | |
166 | 189 |
quatMultiply(output,tmp[0],tmp[1],tmp[2],tmp[3],-qx,-qy,-qz,qw); |
167 | 190 |
} |
168 | 191 |
|
192 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
|
193 |
// rotate (x1,x2,x3,x4) by quat ( i.e. return quat*vector*(quat^-1) ) |
|
194 |
|
|
195 |
public static void rotateVectorByQuat(float[] output, float x, float y, float z, float w, float[] quat) |
|
196 |
{ |
|
197 |
float[] tmp = new float[4]; |
|
198 |
|
|
199 |
float qx = quat[0]; |
|
200 |
float qy = quat[1]; |
|
201 |
float qz = quat[2]; |
|
202 |
float qw = quat[3]; |
|
203 |
|
|
204 |
quatMultiply(tmp,qx,qy,qz,qw,x,y,z,w); |
|
205 |
quatMultiply(output,tmp[0],tmp[1],tmp[2],tmp[3],-qx,-qy,-qz,qw); |
|
206 |
} |
|
207 |
|
|
169 | 208 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
170 | 209 |
// rotate vec by quat ( i.e. return quat*vector*(quat^-1) ) |
171 | 210 |
|
Also available in: Unified diff
speedup