The Ecere SDK - sdk/blob - ecere/src/gfx/3D/Quaternion.ec

   1 namespace gfx3D;
   2
   3 import "Display"
   4
   5 public struct Euler
   6 {
   7    Degrees yaw, pitch, roll;
   8    property Quaternion
   9    {
  10       set
  11       {
  12          double y = 2 * ( value.y*value.z + value.w*value.x );
  13          if(fabs(y) <= 1.0 - 0.000005)
  14          {
  15             double x =     2 * ( value.x*value.z - value.w*value.y );
  16             double z = 1 - 2 * ( value.x*value.x + value.y*value.y );
  17             Angle yaw = -atan2(x, z);
  18             Angle pitch = atan2(y, sqrt(x * x + z * z));
  19             double sYaw = sin( yaw / 2 );
  20             double cYaw = cos( yaw / 2 );
  21             double sPitch = sin( pitch / 2 );
  22             double cPitch = cos( pitch / 2 );
  23             Quaternion yp = { cPitch * cYaw, sPitch * cYaw, cPitch * sYaw, sPitch * sYaw };
  24             double rollW = yp.w * value.w + yp.x * value.x + yp.y * value.y + yp.z * value.z;
  25             double rollZ = yp.w * value.z + yp.x * value.y - yp.y * value.x - yp.z * value.w;
  26
  27             this.yaw = yaw;
  28             this.pitch = pitch;
  29             this.roll = atan2(rollZ, rollW) * 2;
  30          }
  31          else
  32          {
  33             // 90 degrees pitch case
  34             double sin45 = sin(Pi/4);
  35             double yawW = sin45 * value.w + sin45 * value.x;
  36             double yawY = sin45 * value.y + sin45 * value.z;
  37
  38             this.yaw = atan2(yawY, yawW) * 2;
  39             this.pitch = Pi/2;
  40             this.roll = 0;
  41          }
  42       }
  43       get
  44       {
  45          double sYaw   = sin( yaw   / 2 );
  46          double cYaw   = cos( yaw   / 2 );
  47          double sPitch = sin( pitch / 2 );
  48          double cPitch = cos( pitch / 2 );
  49          double sRoll  = sin( roll  / 2 );
  50          double cRoll  = cos( roll  / 2 );
  51          Quaternion yp = { cPitch * cYaw, sPitch * cYaw, cPitch * sYaw, sPitch * sYaw };
  52
  53          value.w = yp.w * cRoll - yp.z * sRoll;
  54          value.x = yp.x * cRoll - yp.y * sRoll;
  55          value.y = yp.y * cRoll + yp.x * sRoll;
  56          value.z = yp.z * cRoll + yp.w * sRoll;
  57       }
  58    };
  59
  60    void Add(Euler e1, Euler e2)
  61    {
  62       yaw   = e1.yaw   + e2.yaw;
  63       pitch = e1.pitch + e2.pitch;
  64       roll  = e1.roll  + e2.roll;
  65    }
  66 };
  67
  68 public struct Quaternion
  69 {
  70    double w, x, y, z;
  71
  72    void Identity(void)
  73    {
  74       x = y = z = 0;
  75       w = 1;
  76    }
  77
  78    void Normalize(Quaternion source)
  79    {
  80       double m = sqrt(source.x * source.x +
  81                       source.y * source.y +
  82                       source.z * source.z +
  83                       source.w * source.w);
  84
  85       if(m)
  86       {
  87          x = (double)(source.x/m);
  88          y = (double)(source.y/m);
  89          z = (double)(source.z/m);
  90          w = (double)(source.w/m);
  91       }
  92       else
  93          w = x = y = z = 0;
  94    }
  95
  96    void Multiply(Quaternion q1, Quaternion q2)
  97    {
  98       w = q1.w * q2.w - q2.x * q1.x - q1.y * q2.y - q1.z * q2.z;
  99       x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
 100       y = q1.w * q2.y + q2.w * q1.y + q1.z * q2.x - q1.x * q2.z;
 101       z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
 102    }
 103
 104    void Divide(Quaternion q1, Quaternion q2)
 105    {
 106       w =  q2.w * q1.w + q2.x * q1.x + q2.y * q1.y + q2.z * q1.z;
 107       x =  q2.w * q1.x - q2.x * q1.w + q2.y * q1.z - q2.z * q1.y;
 108       y =  q2.w * q1.y - q2.x * q1.z - q2.y * q1.w + q2.z * q1.x;
 109       z =  q2.w * q1.z + q2.x * q1.y - q2.y * q1.x - q2.z * q1.w;
 110    }
 111
 112    void RotationAxis(Vector3D axis, Degrees angle)
 113    {
 114       double sa = sin( angle / 2 );
 115       double ca = cos( angle / 2 );
 116
 117       x = axis.x * sa;
 118       y = axis.y * sa;
 119       z = axis.z * sa;
 120       w = ca;
 121    }
 122
 123    void RotationYawPitchRoll(Euler euler)
 124    {
 125       Quaternion rotation, result;
 126
 127       result.Yaw(euler.yaw);
 128       rotation.Pitch(euler.pitch);
 129       Multiply(rotation, result);
 130       rotation.Roll(euler.roll);
 131       result.Multiply(rotation, this);
 132       Normalize(result);
 133    }
 134
 135    void RotationDirection(Vector3D direction)
 136    {
 137       Angle yaw = -atan2(direction.x, direction.z);
 138       Angle pitch = atan2(direction.y, sqrt(direction.x * direction.x + direction.z * direction.z));
 139       YawPitch(yaw, pitch);
 140    }
 141
 142    void RotationMatrix(Matrix m)
 143    {
 144       double t = m.m[0][0] + m.m[1][1] + m.m[2][2] + 1.0;
 145       if(t > 0)
 146       {
 147          double s = sqrt(t) * 2;
 148
 149          w = (double) (0.25f * s);
 150          x = (double) (( m.m[2][1] - m.m[1][2] ) / s);
 151          y = (double) (( m.m[0][2] - m.m[2][0] ) / s);
 152          z = (double) (( m.m[1][0] - m.m[0][1] ) / s);
 153
 154       }
 155       else
 156       {
 157          double q[3];
 158          double s;
 159
 160          int i = 0,j,k;
 161          int nxt[3] = {1,2,0};
 162          if(m.m[1][1] > m.m[0][0]) i = 1;
 163          if(m.m[2][2] > m.m[i][i]) i = 2;
 164          j = nxt[i];
 165          k = nxt[j];
 166          s = sqrt(m.m[i][i] - (m.m[j][j] + m.m[k][k]) + 1.0) * 2;
 167
 168          w = (double) ((m.m[k][j] - m.m[j][k]) / s);
 169
 170          q[i] = (double) (0.25f * s);
 171          q[j] = (double) ((m.m[j][i] - m.m[i][j]) / s);
 172          q[k] = (double) ((m.m[k][i] - m.m[i][k]) / s);
 173
 174          x = q[0];
 175          y = q[1];
 176          z = q[2];
 177       }
 178    }
 179
 180    #define DELTA 0
 181
 182    void Slerp(Quaternion from, Quaternion to, float t)
 183    {
 184       double to1[4];
 185       double omega, cosom, sinom, scale0, scale1;
 186
 187       cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
 188
 189       if ( cosom < 0.0 )
 190       {
 191          cosom = -cosom;
 192          to1[0] = -to.x;
 193          to1[1] = -to.y;
 194          to1[2] = -to.z;
 195          to1[3] = -to.w;
 196       }
 197       else
 198       {
 199          to1[0] = to.x;
 200          to1[1] = to.y;
 201          to1[2] = to.z;
 202          to1[3] = to.w;
 203       }
 204
 205       if ( (1.0 - cosom) > DELTA )
 206       {
 207          omega = acos(cosom);
 208          sinom = sin(omega);
 209          scale0 = sin((1.0 - t) * omega) / sinom;
 210          scale1 = sin(t * omega) / sinom;
 211
 212       }
 213       else
 214       {
 215          scale0 = 1.0 - t;
 216          scale1 = t;
 217       }
 218       x = (double)(scale0 * from.x + scale1 * to1[0]);
 219       y = (double)(scale0 * from.y + scale1 * to1[1]);
 220       z = (double)(scale0 * from.z + scale1 * to1[2]);
 221       w = (double)(scale0 * from.w + scale1 * to1[3]);
 222    }
 223
 224    void Yaw(Degrees angle)
 225    {
 226       double sa = sin( angle / 2 );
 227       double ca = cos( angle / 2 );
 228
 229       x = 0;
 230       y = (double)sa;
 231       z = 0;
 232       w = (double)ca;
 233    }
 234
 235    void YawPitch(Degrees yaw, Degrees pitch)
 236    {
 237       double sYaw   = sin( yaw / 2 );
 238       double cYaw   = cos( yaw / 2 );
 239       double sPitch = sin( pitch / 2 );
 240       double cPitch = cos( pitch / 2 );
 241
 242       w = cPitch * cYaw;
 243       x = sPitch * cYaw;
 244       y = cPitch * sYaw;
 245       z = sPitch * sYaw;
 246    }
 247
 248    void Pitch(Degrees angle)
 249    {
 250       double sa = sin( angle / 2 );
 251       double ca = cos( angle / 2 );
 252
 253       x = (double)sa;
 254       y = 0;
 255       z = 0;
 256       w = (double)ca;
 257    }
 258
 259    void Roll(Degrees angle)
 260    {
 261       double sa = sin( angle / 2 );
 262       double ca = cos( angle / 2 );
 263
 264       x = 0;
 265       y = 0;
 266       z = (double)sa;
 267       w = (double)ca;
 268    }
 269
 270    void RotatePitch(Degrees pitch)
 271    {
 272       Quaternion rotation, result;
 273
 274       rotation.Pitch(pitch);
 275       result.Multiply(rotation, this);
 276       this = result;
 277    }
 278
 279    void RotateYaw(Degrees yaw)
 280    {
 281       Quaternion rotation, result;
 282       rotation.Yaw(yaw);
 283       result.Multiply(rotation, this);
 284       this = result;
 285    }
 286
 287    void RotateRoll(Degrees roll)
 288    {
 289       Quaternion rotation, result;
 290
 291       rotation.Roll(roll);
 292       result.Multiply(rotation, this);
 293       this = result;
 294    }
 295
 296    void RotateYawPitch(Degrees yaw, Degrees pitch)
 297    {
 298       Quaternion rotation, result;
 299       rotation.YawPitch(yaw, pitch);
 300       result.Multiply(rotation, this);
 301       this = result;
 302    }
 303
 304    void ToDirection(Vector3D direction)
 305    {
 306       /*
 307       Vector3Df vector { 0,0,1 };
 308       Matrix mat;
 309       mat.RotationQuaternion(this);
 310       direction.Transform(vector, mat);
 311       */
 312       direction.x = (double)(    2 * ( x*z - w*y ));
 313       direction.y = (double)(    2 * ( y*z + w*x ));
 314       direction.z = (double)(1 - 2 * ( x*x + y*y ));
 315    }
 316
 317    void Inverse(Quaternion source)
 318    {
 319       this = { -source.w, source.x, source.y, source.z };
 320    }
 321 };