Revision 309ba6ea
Added by Leszek Koltunski about 1 year ago
| src/main/java/org/distorted/library/helpers/QuatHelper.java | ||
|---|---|---|
| 143 | 143 |
ret[3] = rw*qw - rz*qz - ry*qy - rx*qx; |
| 144 | 144 |
} |
| 145 | 145 |
|
| 146 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
|
| 147 |
// ret = (qx,qy,qz,qw)*(rx,ry,rz,rw) |
|
| 148 |
|
|
| 149 |
private static void quatMultiply( float[] ret, int index, float qx, float qy, float qz, float qw, float rx, float ry, float rz, float rw ) |
|
| 150 |
{
|
|
| 151 |
ret[index ] = rw*qx - rz*qy + ry*qz + rx*qw; |
|
| 152 |
ret[index+1] = rw*qy + rz*qx + ry*qw - rx*qz; |
|
| 153 |
ret[index+2] = rw*qz + rz*qw - ry*qx + rx*qy; |
|
| 154 |
ret[index+3] = rw*qw - rz*qz - ry*qy - rx*qx; |
|
| 155 |
} |
|
| 156 |
|
|
| 146 | 157 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
| 147 | 158 |
// rotate 'vector' by quat ( i.e. return quat*vector*(quat^-1) ) |
| 148 | 159 |
|
| ... | ... | |
| 189 | 200 |
quatMultiply(output,tmp[0],tmp[1],tmp[2],tmp[3],-qx,-qy,-qz,qw); |
| 190 | 201 |
} |
| 191 | 202 |
|
| 203 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
|
| 204 |
// rotate (x1,x2,x3,x4) by quat ( i.e. return quat*vector*(quat^-1) ) |
|
| 205 |
|
|
| 206 |
public static void rotateVectorByQuat(float[] output, int index, float x, float y, float z, float w, Static4D quat) |
|
| 207 |
{
|
|
| 208 |
float[] tmp = new float[4]; |
|
| 209 |
|
|
| 210 |
float qx = quat.get0(); |
|
| 211 |
float qy = quat.get1(); |
|
| 212 |
float qz = quat.get2(); |
|
| 213 |
float qw = quat.get3(); |
|
| 214 |
|
|
| 215 |
quatMultiply(tmp,qx,qy,qz,qw,x,y,z,w); |
|
| 216 |
quatMultiply(output,index,tmp[0],tmp[1],tmp[2],tmp[3],-qx,-qy,-qz,qw); |
|
| 217 |
} |
|
| 218 |
|
|
| 192 | 219 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
| 193 | 220 |
// rotate (x1,x2,x3,x4) by quat ( i.e. return quat*vector*(quat^-1) ) |
| 194 | 221 |
|
| ... | ... | |
| 205 | 232 |
quatMultiply(output,tmp[0],tmp[1],tmp[2],tmp[3],-qx,-qy,-qz,qw); |
| 206 | 233 |
} |
| 207 | 234 |
|
| 235 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
|
| 236 |
// rotate (x1,x2,x3,x4) by quat ( i.e. return quat*vector*(quat^-1) ) |
|
| 237 |
|
|
| 238 |
public static void rotateVectorByQuat(float[] output, int index, float x, float y, float z, float w, float[] quat) |
|
| 239 |
{
|
|
| 240 |
float[] tmp = new float[4]; |
|
| 241 |
|
|
| 242 |
float qx = quat[0]; |
|
| 243 |
float qy = quat[1]; |
|
| 244 |
float qz = quat[2]; |
|
| 245 |
float qw = quat[3]; |
|
| 246 |
|
|
| 247 |
quatMultiply(tmp,qx,qy,qz,qw,x,y,z,w); |
|
| 248 |
quatMultiply(output,index,tmp[0],tmp[1],tmp[2],tmp[3],-qx,-qy,-qz,qw); |
|
| 249 |
} |
|
| 250 |
|
|
| 208 | 251 |
/////////////////////////////////////////////////////////////////////////////////////////////////// |
| 209 | 252 |
// rotate vec by quat ( i.e. return quat*vector*(quat^-1) ) |
| 210 | 253 |
|
Also available in: Unified diff
major speedup of SolvedObject