Revision a4999c9d
Added by Leszek Koltunski over 2 years 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