[Closed] AngleAxis as Matrix3

looking in the sdk probably

angaxis to quat
quat to matrix3

from deflect.cpp

axis = axis - Point3(0.5f,0.5f,0.5f);
float lsq = LengthSquared(bnorm);
axis -= bnorm*DotProd(axis,bnorm)/lsq;
float angle = PI*(1.0f-rchaos)*randomFloat[(index+57)%500];
Quat quat = QFromAngAxis(angle, axis);
Matrix3 tm;
quat.MakeMatrix(tm);

which is also methodology in the cough halflife2 source code cough

the math (untested)

void AxisAngleQuat(const Point3 &axis, float angle, Quat &q)
{
    float sa, ca;
    
    SinCos(DEG_TO_RAD(angle) * 0.5f, &sa, &ca);
    
    q.x = axis.x * sa;
    q.y = axis.y * sa;
    q.z = axis.z * sa;
    q.w = ca;
}

void QuatToMatrix(const Quat &q, Matrix3& mat)
{
    mat[0][0] = 1.0 - 2.0 * q.y * q.y - 2.0 * q.z * q.z;
    mat[1][0] = 2.0 * q.x * q.y + 2.0 * q.w * q.z;
    mat[2][0] = 2.0 * q.x * q.z - 2.0 * q.w * q.y;

    mat[0][1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z;
    mat[1][1] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.z * q.z;
    mat[2][1] = 2.0f * q.y * q.z + 2.0f * q.w * q.x;

    mat[0][2] = 2.0f * q.x * q.z + 2.0f * q.w * q.y;
    mat[1][2] = 2.0f * q.y * q.z - 2.0f * q.w * q.x;
    mat[2][2] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.y * q.y;
    
    mat[0][3] = 0.0f;
    mat[1][3] = 0.0f;
    mat[2][3] = 0.0f;
}

axis order is probably wrong as it’s for a y is up system, should be easy to tweak for max z is up, the maths doesn’t quite tally with max quat to matrix3 converstion could be they do some kind of orthogonalize/normalize on the result

this gives the correct result

def_visible_primitive(QuatToMatrix3, "QuatToMatrix3");

Value* QuatToMatrix3_cf(Value **arg_list, int count)
{
    check_arg_count(QuatToMatrix3, 1, count);
    Quat q = arg_list[0]->to_quat();
    
    Matrix3 mat(1);

    float    wx, wy, wz;
    float    xx, yy, yz;
    float    xy, xz, zz;
    float    x2, y2, z2;

    x2 = q.x + q.x;
    y2 = q.y + q.y;
    z2 = q.z + q.z;

    xx = q.x * x2;
    xy = q.x * y2;
    xz = q.x * z2;

    yy = q.y * y2;
    yz = q.y * z2;
    zz = q.z * z2;

    wx = q.w * x2;
    wy = q.w * y2;
    wz = q.w * z2;

    mat[0][0] = 1.0f - ( yy + zz );
    mat[0][1] = xy - wz;
    mat[0][2] = xz + wy;

    mat[1][0] = xy + wz;
    mat[1][1] = 1.0f - ( xx + zz );
    mat[1][2] = yz - wx;

    mat[2][0] = xz - wy;
    mat[2][1] = yz + wx;
    mat[2][2] = 1.0f - ( xx + yy );

    mat[3][0] = 0.0f;
    mat[3][1] = 0.0f;
    mat[3][2] = 0.0f;

    return new Matrix3Value(mat);
}

Nope
Works for unitary vectors, as the wikipedia Matrix I had, but really don’t know what Max does with non unitary vectors.

looking at the sdk source code, I imagine the angleaxis to quaterion conversion normalizes the input axis first…

void AxisAngleQuat(const Point3 &axis, float angle, Quat &q)
{
    float sa, ca;

    SinCos(DEG_TO_RAD(angle) * 0.5f, &sa, &ca);

    axis = Normalize(axis);
    q.x = axis.x * sa;
    q.y = axis.y * sa;
    q.z = axis.z * sa;
    q.w = ca;
}

This is what I have so far (Test rotation at the end)

global vec3D, angleAxis3D, quat3D, matrix3D

struct vec3D
(
   p = [0,0,0],
   x = p[1] as double, -- p.x
   y = p[2] as double, -- p.y
   z = p[3] as double, -- p.z
   fn setX k = (x = k as double),
   fn setY k = (y = k as double),
   fn setZ k = (z = k as double),
   fn fromVector v = (x = v.x as double; y = v.y as double; z = v.z as double; this), -- Assign a Point3
   fn toVector = [x, y, z],      -- convert to Point3
   fn fromArray ar = (x = ar[1] as double; y = ar[2] as double; z = ar[3] as double; this),
   fn toArray = #(x,y,z),
   fn length2 = x*x + y*y + z*z,   -- length squared
   fn length = sqrt (length2()),      -- length
   fn normalizeExt = (norm = length(); if norm != 0 then (vec3D x:(x/norm) y:(y/norm) z:(z/norm)) else (vec3D [0,0,1])),   -- get normalized vector
   fn normalize = (v = normalizeExt(); x = v.x; y = v.y; z = v.z; this),   -- set vector to its normalized version
   fn escalarExt k = (vec3D x:(x*k) y:(y*k) z:(z*k)),   -- get vector multiplied by a scalar
   fn escalar k = (x *= k; y *= k; z *= k; this),   -- set vector to its value multiplied by a scalar
   fn sumExt v =(vec3D x:(x+v.x) y:(y+v.y) z:(z+v.z)),   -- get vector by adding this to an external vector (works with Point3)
   fn sum v =(x += v.x; y += v.y; z += v.z; this),   -- set vector to its sum with an external vector (works with Point3)
   fn dot v = x*v.x + y*v.y + z*v.z,      -- dot product (works with Point3)
   fn cross v =   -- cross product with external vector (works with Point3)
   (
      x1 = y*v.z - z*v.y
      y1 = z*v.x - x*v.z
      z1 = x*v.y - y*v.x
      vec3D x:x1 y:y1 z:z1
   ),
   fn multMatrixExt tm =   -- get vector by multiplying this by a Matrix (works with Matrix3 and with MAtrix3D)
   (
      x1 = (x*tm.row1.x + y*tm.row2.x + z*tm.row3.x) + tm.row4.x
      y1 = (x*tm.row1.y + y*tm.row2.y + z*tm.row3.y) + tm.row4.y
      z1 = (x*tm.row1.z + y*tm.row2.z + z*tm.row3.z) + tm.row4.z
      vec3D x:x1 y:y1 z:z1
   ),
   fn multMatrix tm =   -- set vector to its value multiplied by a Matrix (works with Matrix3 and with MAtrix3D)
   (
      v = multMatrixExt tm
      x = v.x; y = v.y; z = v.z
      this
   ),
   fn toAngleAxis ang = angleAxis3D angle:ang axis:this,
   fn toQuat ang =   -- Convert this to Quaternion (quat3D) given an input angle in degrees
   (
      ang *= 0.5 as double
      sa = sin ang
      ca = cos ang
      q = quat3D()
      q.x = x * sa;
      q.y = y * sa;
      q.z = z * sa;
      q.w = ca;
      q
   ),
   fn rotMatrix ang =   --   Get rotation Matrix3D from this vector and a rotation value in degrees 
   (
      q = toQuat ang
      q.toMatrix()
   )
)

struct angleAxis3D
(
   a = (angleAxis 0 [0,0,1]),
   angle = a.angle as double,
   axis = vec3D a.axis,
   fn fromAngleAxis ang = (angle = ang.angle as double; axis = vec3D ang.axis; this),
   fn toAngleAxis = (angleAxis angle (axis.toVector())), 
   fn fromArray ar =
   (
      if ar.count == 2 then
      (
         angle = ar[1] as double; axis = vec3D ar[2]; this
      )
      else
      (
         angle = ar[1] as double; axis = vec3D x:(ar[2] as double) y:(ar[3] as double) z:(ar[4] as double) ; this
      )
   ), 
   fn toArray = #(angle, axis.x, axis.y, axis.z), 
   fn toArray2 = #(angle, axis.toVector()), 
   fn fromQuat q = (if isKindOf q quat then (quat3D q).toAngleAxis() else q.toAngleAxis()), 
   fn toQuat =
   (
      ang = angle * 0.5 as double
      sa = sin ang
      ca = cos ang
      q = quat3D()
      q.x = axis.x * sa;
      q.y = axis.y * sa;
      q.z = axis.z * sa;
      q.w = ca;
      q
   ),
   fn fromMatrix tm = ((quat3D()).fromMatrix tm).toAngleAxis(),
   fn toMatrix =
   (
      (toQuat()).toMatrix()
   )
)

struct quat3D
(
   qt = (quat 0 0 0 1),
   x = qt.x as double,
   y = qt.y as double,
   z = qt.z as double,
   w = qt.w as double,
   fn Norm2 = (x*x + y*y + z*z + w*w),
   fn Norm = sqrt (Norm2()),
   fn NormalizeExt = (n = Norm(); qu = quat3D x:(x/n) y:(y/n) z:(z/n) w:(w/n)),
   fn Normalize = (n = Norm(); x /= n; y /= n; z /= n; w /= n; this),
   fn angle = (n = norm(); 2 * acos (w/n)),
   fn axis = 
   (
      qu = NormalizeExt()
      s = sqrt(1-qu.w*qu.w)
      if (s < 1d-009) then     --   test to avoid division by zero, (s is always positive due to sqrt)
      (                        --   if s is close to zero then the direction of axis is not important
         x1 = qu.x; y1 = qu.y; z1 = qu.z      --   if it is important that axis is normalised then replace with x=y=1; z=0;
      )
      else
      (
         x1 = qu.x / s; y1 = qu.y / s; z1 = qu.z / s      --   de-normalize axis
      )
      (vec3D x:x1 y:y1 z:z1)
   ),
   fn fromQuat q = (x = q.x as double; y = q.y as double; z = z.q as double; w = q.w as double; this),
   fn toQuat = (quat x y z w),
   fn fromArray ar = (x = ar[1] as double; y = ar[2] as double; z = ar[3] as double; w = ar[4] as double; this), 
   fn toArray = #(x, y, z, w), 
   fn fromAngleAxis ang =
   (
      local qu
      if isKindOf ang angleAxis then
      (
         qu = (angleAxis3D ang).toQuat()
      )
      else
      (
         qu = ang.toQuat()
      )
      x = qu.x; y = qu.y; z = qu.z; w = qu.w; this
   ), 
   fn toAngleAxis =   
   (
      a = angleAxis3D angle:(this.angle()) axis:(this.axis())
   ),
   fn toMatrix =
   (
      Nq = Norm2();
      s = if (Nq > 0.0) then (2.0 / Nq) else (0.0d0)
      xs = x*s; ys = y*s; zs = z*s;
      wx = w*xs; wy = w*ys; wz = w*zs;
      xx = x*xs; xy = x*ys; xz = x*zs;
      yy = y*ys; yz = y*zs; zz = z*zs;
      row1X = 1.0 - (yy + zz); row2X = xy + wz; row3X = xz - wy;
      row1Y = xy - wz; row2Y = 1.0 - (xx + zz); row3Y = yz + wx;
      row1Z = xz + wy; row2Z = yz - wx; row3Z = 1.0 - (xx + yy);
         
      matrix3D row1:(vec3D x:row1X y:row1Y z:row1Z) row2:(vec3D x:row2X y:row2Y z:row2Z) row3:(vec3D x:row3X y:row3Y z:row3Z) row4:(vec3D [0,0,0])
   ),
   fn fromMatrix tm =
   (
      /* This algorithm avoids near-zero divides by looking for a large component
       * — first w, then x, y, or z. When the trace is greater than zero,
       * |w| is greater than 1/2, which is as small as a largest component can be.
       * Otherwise, the largest diagonal entry corresponds to the largest of |x|,
       * |y|, or |z|, one of which must be larger than |w|, and at least 1/2.
      */
      local mat = if isKindof tm matrix3 then (matrix3D tm).toArray() else tm.toArray()
      qu = quat3D()
      local tr, s;
      tr = mat[1][1] + mat[2][2]+ mat[3][3];
      
      local mat44 = 1.0   -- mat[4][4] ???
      
      if (tr >= 0.0) then
      (
         s = sqrt(tr + mat44);
         qu.w = s*0.5;
         s = 0.5 / s;
         qu.x = (mat[3][2] - mat[2][3]) * s;
         qu.y = (mat[1][3] - mat[3][1]) * s;
         qu.z = (mat[2][1] - mat[1][2]) * s;
      )
      else
      (
         h = 1
         if (mat[2][2] > mat[1][1]) do h = 2;
         if (mat[3][3] > mat[h][h]) do h = 3;
         case h of
         (
            1:   (
                  s = sqrt( (mat[1][1] - (mat[2][2]+mat[3][3])) + mat44 );
                  qu.x = s*0.5;
                  s = 0.5 / s;
                  qu.y = (mat[1][2] + mat[2][1]) * s;
                  qu.z = (mat[3][1] + mat[1][3]) * s;
                  qu.w = (mat[3][2] - mat[2][3]) * s;
               )
            2:   (
                  s = sqrt( (mat[2][2] - (mat[3][3]+mat[1][1])) + mat44 );
                  qu.y = s*0.5;
                  s = 0.5 / s;
                  qu.z = (mat[2][3] + mat[3][2]) * s;
                  qu.x = (mat[1][2] + mat[2][1]) * s;
                  qu.w = (mat[1][3] - mat[3][1]) * s;
               )
            3:   (
                  s = sqrt( (mat[3][3] - (mat[1][1]+mat[2][2])) + mat44 );
                  qu.z = s*0.5;
                  s = 0.5 / s;
                  qu.x = (mat[3][1] + mat[1][3]) * s;
                  qu.y = (mat[2][3] + mat[3][2]) * s;
                  qu.w = (mat[2][1] - mat[1][2]) * s;
               )
         )
      )
      
      if (mat44 != 1.0) do
      (
         s = 1.0/sqrt(mat44);
         qu.w *= s; qu.x *= s; qu.y *= s; qu.z *= s;
      )
      x = qu.x; y = qu.y; z = qu.z; w = qu.w
      this
   )
)

struct matrix3D
(
   tm = matrix3 1,
   row1 = vec3D tm.row1,
   row2 = vec3D tm.row2,
   row3 = vec3D tm.row3,
   row4 = vec3D tm.row4,
   fn setRow1 v = (row1 = vec3D v),
   fn setRow2 v = (row2 = vec3D v),
   fn setRow3 v = (row3 = vec3D v),
   fn setRow4 v = (row4 = vec3D v),
   fn fromMatrix3 tm1 = (row1 = vec3D tm1.row1; row2 = vec3D tm1.row2; row3 = vec3D tm1.row3; row4 = vec3D tm1.row4; this), -- Assign a matrix3 value
   fn toMatrix3 = matrix3 (row1.toVector()) (row2.toVector()) (row3.toVector()) (row4.toVector()),   -- Convert to matrix3 value
   fn toArray =
   (
      ta = #(#(),#(),#(),#())
      ta[1] = row1.toArray(); ta[2] = row2.toArray(); ta[3] = row3.toArray(); ta[4] = row4.toArray(); ta
   ),
   fn fromArray ar =
   (
      row1 = (vec3D()).fromArray ar[1]; row2 = (vec3D()).fromArray ar[2]; row3 = (vec3D()).fromArray ar[3]; row4 = (vec3D()).fromArray ar[4]; this
   )
)
-- Test
(
   local v = [1,2,3] 
   local q = (angleaxis 30 v) as quat
   local v1 = vec3D v
      
   format "3dsMax Quat: %
" q
   format "Double Quat: %
" (v1.toQuat 30)
      
   format "3dsMax Matrix from Quat: %
" (q as matrix3)
   format "Double Matrix from Quat (1): %
" (v1.rotMatrix 30)
   format "Double Matrix from Quat (2): %
" ((v1.toQuat 30).toMatrix())
)

Updated!

It must be very slow… and it has to use a lot of memory. Struct creation is very expensive.

struct float3
(
   x, y, z,
   fn vcross v = 
   (
      x1 = y*v.z - z*v.y
      y1 = z*v.x - x*v.z
      z1 = x*v.y - y*v.x
      float3 x:x1 y:y1 z:z1
   )
)
(
   t = timestamp()
   h = heapfree
   v0 = float3 1 2 3
   for k=1 to 100000 do
   (
      v1 = float3 k 2 3
      v0.vcross v1
   )
   format "time:% heap:%
" (timestamp() - t) (h - heapfree)
)
(
   t = timestamp()
   h = heapfree
   v0 = point3 1 2 3
   for k=1 to 100000 do
   (
      v1 = point3 k 2 3
      cross v0 v1
   )
   format "time:% heap:%
" (timestamp() - t) (h - heapfree)
)

from the Ken Shoemake paper (referenced in the sdk documentation)

typedef struct {float x,y,z,w;} Quat;
typedef float HMatrix[4][4];
#define X 0
#define Y 1
#define Z 2
#define W 3
/* Return quaternion product qL * qR. */
Quat Qt_Mul(Quat qL, Quat qR)
{
Quat qq;
qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
return (qq);
}
/* Return norm of quaternion, the sum of the squares of the components. */
#define Qt_Norm(q) ((q).x*(q).x + (q).y*(q).y + (q).z*(q).z + (q).w*(q).w)
/* Construct rotation matrix from (possibly non-unit) quaternion.
* Assumes matrix is used to multiply column vector on the left:
* vnew = mat vold. Works correctly for right-handed coordinate system
* and right-handed rotations. */
void Qt_ToMatrix(Quat q, HMatrix mat)
{
double Nq = Qt_Norm(q);
double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
double xs = q.x*s, ys = q.y*s, zs = q.z*s;
double wx = q.w*xs, wy = q.w*ys, wz = q.w*zs;
double xx = q.x*xs, xy = q.x*ys, xz = q.x*zs;
double yy = q.y*ys, yz = q.y*zs, zz = q.z*zs;
mat[X][X] = 1.0 - (yy + zz); mat[Y][X] = xy + wz; mat[Z][X] = xz - wy;
mat[X][Y] = xy - wz; mat[Y][Y] = 1.0 - (xx + zz); mat[Z][Y] = yz + wx;
mat[X][Z] = xz + wy; mat[Y][Z] = yz - wx; mat[Z][Z] = 1.0 - (xx + yy);
mat[X][W] = mat[Y][W] = mat[Z][W] = 0.0;
mat[W][X] = mat[W][Y] = mat[W][Z] = 0.0;
mat[W][W] = 1.0;
}

normalizes the quat before calculate the rotation matrix

That is!! Thanks!!

In fact, the “System.Windows.Media.Media3D” has most of what I need. It miss converssions.

   dotNet.loadAssembly "PresentationCore.dll"

   fn Matrix3ToArray tm =
   (
      local ar = #(#(),#(),#(),#())
      for i = 1 to 4 do
      (
         for j = 1 to 3 do
         (
            ar[i][j] = tm[i][j] as double
         )
         ar[i][4] = 0 as double
      )
      ar[4][4] = 1 as double
      ar
   )
   
   fn Matrix3ToMAtrix3D tm =
   (
      local a = Matrix3ToArray tm
      tm = dotnetObject "System.Windows.Media.Media3D.Matrix3D"          a[1][1] a[1][2] a[1][3] a[1][4]\                                             
                                                                        a[2][1] a[2][2] a[2][3] a[2][4]\
                                                                        a[3][1] a[3][2] a[3][3] a[3][4]\
                                                                        a[4][1] a[4][2] a[4][3] a[4][4]
   )
   p1 = dotnetObject "System.Windows.Media.Media3D.Point3D" 1 1 1
   tm = (matrix3 [2.91815,0.427563,0.0545673] [-0.216218,0.632014,-0.773141] [-0.241499,0.64506,0.538494] [-136.121,124.7,0])
   tm1 = Matrix3ToMAtrix3D tm
   tm1.toString()
   [1,1,1] * tm
   p2 = tm1.transform p1   --   This two ones are equivalent
   p3 = p1.multiply p1 tm1
   p2.toString()
   p3.toString()
   
   inverse tm
   tm1.invert()   --   Invert matrix
   tm1.toString()