Revision 309ba6ea
Added by Leszek Koltunski 12 months 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