Project

General

Profile

« Previous | Next » 

Revision a4999c9d

Added by Leszek Koltunski over 1 year ago

speedup

View differences:

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