/**************************************************************************
 *{@C
 *      Copyright:      1988-2020 Paul Obermeier (obermeier@poSoft.de)
 *
 *      Module:         Utilities
 *      Filename:       UT_VecTfm.c
 *
 *      Author:         Paul Obermeier
 *
 *      Description:    Matrix generation and evaluation for two and three
 *                      dimensional translation, rotation and scaling.
 *                      The result parameters must not point to the same
 *                      locations in memory as the input parameters.
 *
 *      Additional documentation:
 *                      None.
 *
 *  Exported functions:
 *                      UT_TfmBuildTrans2D
 *                      UT_TfmBuildTrans3D
 *                      UT_DTfmBuildTrans3D
 *                      UT_DTfmBuildDTrans3D
 *                      UT_TfmBuildScale2D
 *                      UT_TfmBuildScale3D
 *                      UT_TfmBuildLookAt3D
 *                      UT_TfmBuildRot2D
 *                      UT_TfmBuildRot3D
 *                      UT_DTfmBuildRot3D
 *                      UT_DTfmBuildDRot3D
 *                      UT_TfmBuildRotArb3D
 *                      UT_TfmBuildRotArbLos3D
 *                      UT_TfmConcat2D
 *                      UT_TfmConcat3D
 *                      UT_DTfmConcat3D
 *                      UT_TfmConcatTrans2D
 *                      UT_TfmConcatTrans3D
 *                      UT_DTfmConcatTrans3D
 *                      UT_DTfmConcatDTrans3D
 *                      UT_TfmConcatScale2D
 *                      UT_TfmConcatScale3D
 *                      UT_TfmConcatRot2D
 *                      UT_TfmConcatRot3D
 *                      UT_DTfmConcatRot3D
 *                      UT_DTfmConcatDRot3D
 *                      UT_TfmInvert2D
 *                      UT_TfmInvert3D
 *                      UT_TfmInitCoord2D
 *                      UT_TfmInitCoord3D
 *                      UT_TfmCoord2D
 *                      UT_TfmCoord3D
 *                      UT_TfmApply2D
 *                      UT_TfmApplyNormal3D
 *
 **************************************************************************/

#include <stdio.h>
#include <math.h>

#include "UT_Compat.h"
#include "UT_Const.h"
#include "UT_Macros.h"

#include "UT_Vector.h"

/* The identical transformation in two and three dimensions */

const UT_Tfm2D UT_TfmId2D = {
    {1.0, 0.0},
    {0.0, 1.0},
    {0.0, 0.0}
};

const UT_Tfm3D UT_TfmId3D = {
    {1.0, 0.0, 0.0},
    {0.0, 1.0, 0.0},
    {0.0, 0.0, 1.0},
    {0.0, 0.0, 0.0}
};

/* Two and three dimensional "world" coordinate systems */

const UT_CoSys2D UT_TfmWorldCoSys2D = {
    {0.0, 0.0},
    {
        {1.0, 0.0},
        {0.0, 1.0}
    }
};

const UT_CoSys3D UT_TfmWorldCoSys3D = {
    {0.0, 0.0, 0.0},
    {
        {1.0, 0.0, 0.0},
        {0.0, 1.0, 0.0},
        {0.0, 0.0, 1.0}
    }
};

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildTrans2D
 *
 *      Usage:          Build a 2D transformation matrix for translation.
 *
 *      Synopsis:       void UT_TfmBuildTrans2D (const UT_Vec2D v, UT_Tfm2D T)
 *
 *      Description:    The coefficients of T are set so that T represents
 *                      a translation by v.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmBuildTrans2D (const UT_Vec2D v, UT_Tfm2D T)
{
    T[0][0] = 1.0;
    T[0][1] = 0.0;
    T[1][0] = 0.0;
    T[1][1] = 1.0;
    T[2][0] = v[0];
    T[2][1] = v[1];
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildTrans3D
 *
 *      Usage:          Build a 3D transformation matrix for translation.
 *
 *      Synopsis:       void UT_TfmBuildTrans3D (const UT_Vec3D v, UT_Tfm3D T)
 *
 *      Description:    The coefficients of T are set so that T represents
 *                      a translation by v.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmBuildTrans3D (const UT_Vec3D v, UT_Tfm3D T)
{
    T[0][0] = 1.0;
    T[0][1] = 0.0;
    T[0][2] = 0.0;
    T[1][0] = 0.0;
    T[1][1] = 1.0;
    T[1][2] = 0.0;
    T[2][0] = 0.0;
    T[2][1] = 0.0;
    T[2][2] = 1.0;
    T[3][0] = v[0];
    T[3][1] = v[1];
    T[3][2] = v[2];
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmBuildTrans3D
 *
 *      Usage:          Build a 3D transformation matrix for translation.
 *
 *      Synopsis:       void UT_DTfmBuildTrans3D (const UT_DVec3D v, UT_Tfm3D T)
 *
 *      Description:    The coefficients of T are set so that T represents
 *                      a translation by v.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmBuildTrans3D (const UT_DVec3D v, UT_Tfm3D T)
{
    T[0][0] = 1.0;
    T[0][1] = 0.0;
    T[0][2] = 0.0;
    T[1][0] = 0.0;
    T[1][1] = 1.0;
    T[1][2] = 0.0;
    T[2][0] = 0.0;
    T[2][1] = 0.0;
    T[2][2] = 1.0;
    T[3][0] = v[0];
    T[3][1] = v[1];
    T[3][2] = v[2];
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmBuildDTrans3D
 *
 *      Usage:          Build a 3D transformation matrix for translation.
 *
 *      Synopsis:       void UT_DTfmBuildDTrans3D (const UT_DVec3D v, UT_DTfm3D T)
 *
 *      Description:    The coefficients of T are set so that T represents
 *                      a translation by v.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmBuildDTrans3D (const UT_DVec3D v, UT_DTfm3D T)
{
    T[0][0] = 1.0;
    T[0][1] = 0.0;
    T[0][2] = 0.0;
    T[1][0] = 0.0;
    T[1][1] = 1.0;
    T[1][2] = 0.0;
    T[2][0] = 0.0;
    T[2][1] = 0.0;
    T[2][2] = 1.0;
    T[3][0] = v[0];
    T[3][1] = v[1];
    T[3][2] = v[2];
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildRot2D
 *
 *      Usage:          Build a 2D transformation matrix for rotation.
 *
 *      Synopsis:       void UT_TfmBuildRot2D (Float32 ang, UT_Tfm2D T)
 *
 *      Description:    The coefficients of v represent angles for a rotation
 *                      around the coordinate axes. T is set so that it
 *                      represents a rotation around X (0) and around Y (1).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmBuildRot2D (Float32 ang, UT_Tfm2D T)
{
    Float32 s, c;

    s = -sin (ang);
    c = cos (ang);
    T[0][0] = c;
    T[0][1] = -s;
    T[1][0] = s;
    T[1][1] = c;
    T[2][0] = 0.0;
    T[2][1] = 0.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildRot3D
 *
 *      Usage:          Build a 3D transformation matrix for rotation.
 *
 *      Synopsis:       void UT_TfmBuildRot3D (const UT_Vec3D v, UT_Tfm3D T)
 *
 *      Description:    The coefficients of v represent angles for a rotation
 *                      around the coordinate axes. T is set so that it
 *                      represents a rotation around X (0), around Y (1)
 *                      and then around the Z (2) axis.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmBuildRot3D (const UT_Vec3D v, UT_Tfm3D T)
{
    Float32 c0, c1, c2, s0, s1, s2;

    c0 =  cos (v[0]);
    c1 =  cos (v[1]);
    c2 =  cos (v[2]);
    s0 = -sin (v[0]);
    s1 = -sin (v[1]);
    s2 = -sin (v[2]);
    T[0][0] = c1 * c2;
    T[0][1] = -c1 * s2;
    T[0][2] = s1;
    T[1][0] = s0 * s1 * c2 + c0 * s2;
    T[1][1] = -s0 * s1 * s2 + c0 * c2;
    T[1][2] = -s0 * c1;
    T[2][0] = -c0 * s1 * c2 + s0 * s2;
    T[2][1] = c0 * s1 * s2 + s0 * c2;
    T[2][2] = c0 * c1;
    T[3][0] = 0.0;
    T[3][1] = 0.0;
    T[3][2] = 0.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmBuildRot3D
 *
 *      Usage:          Build a 3D transformation matrix for rotation.
 *
 *      Synopsis:       void UT_DTfmBuildRot3D (const UT_DVec3D v, UT_Tfm3D T)
 *
 *      Description:    The coefficients of v represent angles for a rotation
 *                      around the coordinate axes. T is set so that it
 *                      represents a rotation around X (0), around Y (1)
 *                      and then around the Z (2) axis.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmBuildRot3D (const UT_DVec3D v, UT_Tfm3D T)
{
    Float64 c0, c1, c2, s0, s1, s2;

    c0 =  cos (v[0]);
    c1 =  cos (v[1]);
    c2 =  cos (v[2]);
    s0 = -sin (v[0]);
    s1 = -sin (v[1]);
    s2 = -sin (v[2]);
    T[0][0] = c1 * c2;
    T[0][1] = -c1 * s2;
    T[0][2] = s1;
    T[1][0] = s0 * s1 * c2 + c0 * s2;
    T[1][1] = -s0 * s1 * s2 + c0 * c2;
    T[1][2] = -s0 * c1;
    T[2][0] = -c0 * s1 * c2 + s0 * s2;
    T[2][1] = c0 * s1 * s2 + s0 * c2;
    T[2][2] = c0 * c1;
    T[3][0] = 0.0;
    T[3][1] = 0.0;
    T[3][2] = 0.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmBuildDRot3D
 *
 *      Usage:          Build a 3D transformation matrix for rotation.
 *
 *      Synopsis:       void UT_DTfmBuildDRot3D (const UT_DVec3D v, UT_DTfm3D T)
 *
 *      Description:    The coefficients of v represent angles for a rotation
 *                      around the coordinate axes. T is set so that it
 *                      represents a rotation around X (0), around Y (1)
 *                      and then around the Z (2) axis.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmBuildDRot3D (const UT_DVec3D v, UT_DTfm3D T)
{
    Float64 c0, c1, c2, s0, s1, s2;

    c0 =  cos (v[0]);
    c1 =  cos (v[1]);
    c2 =  cos (v[2]);
    s0 = -sin (v[0]);
    s1 = -sin (v[1]);
    s2 = -sin (v[2]);
    T[0][0] = c1 * c2;
    T[0][1] = -c1 * s2;
    T[0][2] = s1;
    T[1][0] = s0 * s1 * c2 + c0 * s2;
    T[1][1] = -s0 * s1 * s2 + c0 * c2;
    T[1][2] = -s0 * c1;
    T[2][0] = -c0 * s1 * c2 + s0 * s2;
    T[2][1] = c0 * s1 * s2 + s0 * c2;
    T[2][2] = c0 * c1;
    T[3][0] = 0.0;
    T[3][1] = 0.0;
    T[3][2] = 0.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildRotArb3D
 *
 *      Usage:          Build a 3D transformation matrix for rotation 
 *                      around an axis going through the origin with
 *                      arbitrary direction.
 *
 *      Synopsis:       void UT_TfmBuildRotArb3D
 *                              (const UT_Vec3D v, Float32 ang, UT_Tfm3D T)
 *
 *      Description:    The coefficients of matrix T are set, so that T
 *                      represents a rotation around an axis passing through
 *                      the coordinate origin with direction v.
 *                      The rotation angle "ang", is measured counterclockwise
 *                      looking into the opposite direction of vector v.
 *
 *                      The angle must be specified in radians.
 *                      The length of vector v must be 1.0.
 *
 *                      The formula for rotating about an arbitrary axis can
 *                      be found for example in:
 *                      Akenine-Möller: Real-Time Rendering, 2nd Ed., page 43.
 *
 *      Return value:   None.
 *
 *      See also:       UT_TfmBuildRotArbLos3D
 *
 ***************************************************************************/

void UT_TfmBuildRotArb3D (const UT_Vec3D v, Float32 ang, UT_Tfm3D T)
{
    Float32 c, c1, s;

    s = sin (ang);
    c = cos (ang);
    c1 = 1.0 - c;
    T[0][0] = v[0] * v[0] * c1 + c;
    T[1][0] = v[0] * v[1] * c1 + s * v[2];
    T[2][0] = v[0] * v[2] * c1 - s * v[1];
    T[0][1] = v[0] * v[1] * c1 - s * v[2];
    T[1][1] = v[1] * v[1] * c1 + c;
    T[2][1] = v[1] * v[2] * c1 + s * v[0];
    T[0][2] = v[0] * v[2] * c1 + s * v[1];
    T[1][2] = v[1] * v[2] * c1 - s * v[0];
    T[2][2] = v[2] * v[2] * c1 + c;
    T[3][0] = 0.0;
    T[3][1] = 0.0;
    T[3][2] = 0.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildRotArbLos3D
 *
 *      Usage:          Build a 3D transformation matrix for rotation around
 *                      an axis with arbitrary direction.
 *
 *      Synopsis:       void UT_TfmBuildRotArbLos3D
 *                              (const UT_Vec3D o, const UT_Vec3D v,
 *                               Float32 ang, UT_Tfm3D T)
 *
 *      Description:    The coefficients of matrix "T" are set, so that "T"
 *                      represents a rotation around an axis passing through
 *                      the origin "o" with direction "v".
 *                      The rotation angle "ang", is measured counterclockwise
 *                      looking into the opposite direction of vector "v".
 *
 *                      The angle must be specified in radians.
 *                      The length of vector "v" must be 1.0.
 *
 *                      The formula for rotating about an arbitrary axis can
 *                      be found for example in:
 *                      Akenine-Möller: Real-Time Rendering, 2nd Ed., page 43.
 *
 *      Return value:   None.
 *
 *      See also:       UT_TfmBuildRotArb3D
 *
 ***************************************************************************/

void UT_TfmBuildRotArbLos3D (const UT_Vec3D o, const UT_Vec3D v,
                             Float32 ang, UT_Tfm3D T)
{
    UT_Vec3D ot;
    UT_Tfm3D tmpTfm, rotTfm;

    UT_TfmBuildRotArb3D (v, ang, rotTfm);

    UT_VecScale3D (-1.0, o, ot);
    UT_TfmBuildTrans3D (ot, tmpTfm);

    UT_TfmConcat3D (tmpTfm, rotTfm, T);
    UT_TfmConcatTrans3D (o, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildScale2D
 *
 *      Usage:          Build a 2D transformation matrix for scaling.
 *
 *      Synopsis:       void UT_TfmBuildScale2D (const UT_Vec2D v, UT_Tfm2D T)
 *
 *      Description:    The coefficients of v represent scaling factors
 *                      along the coordinate axes. T is set so that it
 *                      represents the corresponding transformation.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmBuildScale2D (const UT_Vec2D v, UT_Tfm2D T)
{
    T[0][0] = v[0];
    T[0][1] = 0.0;
    T[1][0] = 0.0;
    T[1][1] = v[1];
    T[2][0] = 0.0;
    T[2][1] = 0.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildScale3D
 *
 *      Usage:          Build a 3D transformation matrix for scaling.
 *
 *      Synopsis:       void UT_TfmBuildScale3D (const UT_Vec3D v, UT_Tfm3D T)
 *
 *      Description:    The coefficients of v represent scaling factors
 *                      along the coordinate axes. T is set so that it
 *                      represents the corresponding transformation.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmBuildScale3D (const UT_Vec3D v, UT_Tfm3D T)
{
    T[0][0] = v[0];
    T[0][1] = 0.0;
    T[0][2] = 0.0;
    T[1][0] = 0.0;
    T[1][1] = v[1];
    T[1][2] = 0.0;
    T[2][0] = 0.0;
    T[2][1] = 0.0;
    T[2][2] = v[2];
    T[3][0] = 0.0;
    T[3][1] = 0.0;
    T[3][2] = 0.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmBuildLookAt3D
 *
 *      Usage:          Build a 2D transformation matrix for rotation around an
 *                      axis with arbitrary direction.
 *
 *      Synopsis:       void UT_TfmBuildLookAt3D
 *                              (const UT_Vec3D eyePos, const UT_Vec3D atPos,
 *                               const UT_Vec3D upVec, UT_Tfm3D T)
 *
 *      Description:    The coefficients of matrix T are set to that T
 *                      represents a rotation around an axis passing through
 *                      the coordinate origin with direction v.
 *                      The rotation angle, ang, is measured counterclockwise
 *                      looking into the opposite direction of vector v.
 *                      The length of vector v must be 1.0.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmBuildLookAt3D (const UT_Vec3D eyePos, const UT_Vec3D atPos, 
                          const UT_Vec3D upVec, UT_Tfm3D T)
{
    UT_Vec3D tmpAt;

    /* Compute our new look at vector, which will be
     * the new negative Z axis of our transformed object.
     */
    UT_VecSub3D  (atPos, eyePos, tmpAt);
    UT_VecUnit3D (tmpAt);

    UT_TfmBuildLookAtVec3D (eyePos, tmpAt, upVec, T);
    return;
}

void UT_TfmBuildLookAtVec3D (const UT_Vec3D eyePos, const UT_Vec3D atVec, 
                             const UT_Vec3D upVec, UT_Tfm3D T)
{
    UT_Vec3D tmpAt, tmpUp, xAxis;

    /* Initialize the matrix with the identity transformation. */
    UT_TfmCopy3D (UT_TfmId3D, T);  

    /* Make a useable copy of the current at vector and normalize it. */
    UT_VecCopy3D (atVec, tmpAt);
    UT_VecUnit3D (tmpAt);

    /* Make a useable copy of the current up vector. */
    UT_VecCopy3D (upVec, tmpUp);

    /* Cross product of the new look at vector and the current
     * up vector will produce a vector which is the new
     * positive X axis of our transformed object.
     */
    UT_VecCrossProd3D (tmpAt, tmpUp, xAxis);
    UT_VecUnit3D (xAxis);

    /* Calculate the new up vector, which will be the
     * positive Y axis of our transformed object. Note
     * that it will lie in the same plane as the new
     * look at vector and the old up vector.
     */
    UT_VecCrossProd3D (xAxis, tmpAt, tmpUp);

    /* Account for the fact that the geometry will be defined to
     * point along the negative Z axis.
     */
    UT_VecScale3D (-1.0, tmpAt, tmpAt);

    T[0][0] = xAxis[0];
    T[0][1] = xAxis[1];
    T[0][2] = xAxis[2];
    T[1][0] = tmpUp[0];
    T[1][1] = tmpUp[1];
    T[1][2] = tmpUp[2];
    T[2][0] = tmpAt[0];
    T[2][1] = tmpAt[1];
    T[2][2] = tmpAt[2];
    T[3][0] = eyePos[0];
    T[3][1] = eyePos[1];
    T[3][2] = eyePos[2];
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmConcat2D
 *
 *      Usage:          Concatenate two 2D transformation matrices.
 *
 *      Synopsis:       void UT_TfmConcat2D (UT_Tfm2D A, UT_Tfm2D B, UT_Tfm2D R)
 *
 *      Description:    A transformation R is built so that it has the same
 *                      effect on a vector as sequential application of first
 *                      transformation A and then transformation B (R = AB).
 *                      R may refer to the same memory location as A or B.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmConcat2D (UT_Tfm2D A, UT_Tfm2D B, UT_Tfm2D R)
{
    int   row, col, i;
    UT_Tfm2D atmp, btmp;

    if (R == A) {
        UT_TfmCopy2D (A, atmp);
        A = atmp;
    }
    if (R == B) {
        UT_TfmCopy2D (B, btmp);
        B = btmp;
    }
    for (row = 0; row <= 2; ++row) {
        for (col = 0; col <= 1; ++col) {
            R[row][col] = 0.0;
            for (i = 0; i <= 2; ++i) {
                R[row][col] +=
                    (i < 2? A[row][i]: (row < 2? 0.0: 1.0)) * B[i][col];
            }
        }
    }
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmConcat3D
 *
 *      Usage:          Concatenate two 3D transformation matrices.
 *
 *      Synopsis:       void UT_TfmConcat3D (UT_Tfm3D A, UT_Tfm3D B, UT_Tfm3D R)
 *
 *      Description:    A transformation R is built so that it has the same
 *                      effect on a vector as sequential application of first
 *                      transformation A and then transformation B (R = AB).
 *                      R may refer to the same memory location as A or B.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmConcat3D (UT_Tfm3D A, UT_Tfm3D B, UT_Tfm3D R)
{
    int   row, col, i;
    UT_Tfm3D atmp, btmp;

    if (R == A) {
        UT_TfmCopy3D (A, atmp);
        A = atmp;
    }
    if (R == B) {
        UT_TfmCopy3D (B, btmp);
        B = btmp;
    }
    for (row = 0; row <= 3; ++row) {
        for (col = 0; col <= 2; ++col) {
            R[row][col] = 0.0;
            for (i = 0; i <= 3; ++i) {
                R[row][col] +=
                    (i < 3? A[row][i]: (row < 3? 0.0: 1.0)) * B[i][col];
            }
        }
    }
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmConcat3D
 *
 *      Usage:          Concatenate two 3D transformation matrices.
 *
 *      Synopsis:       void UT_DTfmConcat3D (UT_DTfm3D A, UT_DTfm3D B, UT_DTfm3D R)
 *
 *      Description:    A transformation R is built so that it has the same
 *                      effect on a vector as sequential application of first
 *                      transformation A and then transformation B (R = AB).
 *                      R may refer to the same memory location as A or B.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmConcat3D (UT_DTfm3D A, UT_DTfm3D B, UT_DTfm3D R)
{
    int   row, col, i;
    UT_DTfm3D atmp, btmp;

    if (R == A) {
        UT_DTfmCopy3D (A, atmp);
        A = atmp;
    }
    if (R == B) {
        UT_DTfmCopy3D (B, btmp);
        B = btmp;
    }
    for (row = 0; row <= 3; ++row) {
        for (col = 0; col <= 2; ++col) {
            R[row][col] = 0.0;
            for (i = 0; i <= 3; ++i) {
                R[row][col] +=
                    (i < 3? A[row][i]: (row < 3? 0.0: 1.0)) * B[i][col];
            }
        }
    }
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmConcatTrans2D
 *
 *      Usage:          Concatenate a 2D transformation matrix with a
 *                      translation vector.
 *
 *      Synopsis:       void UT_TfmConcatTrans2D (const UT_Vec2D v, UT_Tfm2D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      translation by v is generated. T and M are
 *                      then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmConcatTrans2D (const UT_Vec2D v, UT_Tfm2D T)
{
    UT_Tfm2D M;

    UT_TfmBuildTrans2D (v, M);
    UT_TfmConcat2D (T, M, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmConcatTrans3D
 *
 *      Usage:          Concatenate a 3D transformation matrix with a
 *                      translation vector.
 *
 *      Synopsis:       void UT_TfmConcatTrans3D (const UT_Vec3D v, UT_Tfm3D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      translation by v is generated. T and M are
 *                      then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmConcatTrans3D (const UT_Vec3D v, UT_Tfm3D T)
{
    UT_Tfm3D M;

    UT_TfmBuildTrans3D (v, M);
    UT_TfmConcat3D (T, M, T);
    return;
}

void UT_TfmMultTrans3D (const UT_Vec3D v, UT_Tfm3D T)
{
    UT_Tfm3D M;

    UT_TfmBuildTrans3D (v, M);
    UT_TfmConcat3D (M, T, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmConcatTrans3D
 *
 *      Usage:          Concatenate a 3D transformation matrix with a
 *                      translation vector.
 *
 *      Synopsis:       void UT_DTfmConcatTrans3D (const UT_DVec3D v, UT_Tfm3D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      translation by v is generated. T and M are
 *                      then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmConcatTrans3D (const UT_DVec3D v, UT_Tfm3D T)
{
    UT_Tfm3D M;
    UT_Vec3D v32;
    v32[0] = v[0];
    v32[1] = v[1];
    v32[2] = v[2];

    UT_TfmBuildTrans3D (v32, M);
    UT_TfmConcat3D (T, M, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmConcatDTrans3D
 *
 *      Usage:          Concatenate a 3D transformation matrix with a
 *                      translation vector.
 *
 *      Synopsis:       void UT_DTfmConcatDTrans3D (const UT_DVec3D v, UT_DTfm3D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      translation by v is generated. T and M are
 *                      then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmConcatDTrans3D (const UT_DVec3D v, UT_DTfm3D T)
{
    UT_DTfm3D M;

    UT_DTfmBuildDTrans3D (v, M);
    UT_DTfmConcat3D (T, M, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmConcatRot2D
 *
 *      Usage:          Concatenate a 2D transformation matrix with a 
 *                      rotation vector.
 *
 *      Synopsis:       void UT_TfmConcatRot2D (Float32 ang, UT_Tfm2D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      rotation, first around the 0, then around the 1
 *                      and then the 2 axis, as specified by v, is generated.
 *                      T and M are then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmConcatRot2D (Float32 ang, UT_Tfm2D T)
{
    UT_Tfm2D M;

    UT_TfmBuildRot2D (ang, M);
    UT_TfmConcat2D (T, M, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmConcatRot3D
 *
 *      Usage:          Concatenate a 3D transformation matrix with a
 *                      rotation vector.
 *
 *      Synopsis:       void UT_TfmConcatRot3D (const UT_Vec3D v, UT_Tfm3D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      rotation, first around the 0, then around the 1
 *                      and then the 2 axis, as specified by v, is generated.
 *                      T and M are then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmConcatRot3D (const UT_Vec3D v, UT_Tfm3D T)
{
    UT_Tfm3D M;

    UT_TfmBuildRot3D (v, M);
    UT_TfmConcat3D (T, M, T);
    return;
}

void UT_TfmMultRot3D (const UT_Vec3D v, UT_Tfm3D T)
{
    UT_Tfm3D M;

    UT_TfmBuildRot3D (v, M);
    UT_TfmConcat3D (M, T, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmConcatRot3D
 *
 *      Usage:          Concatenate a 3D transformation matrix with a
 *                      rotation vector.
 *
 *      Synopsis:       void UT_DTfmConcatRot3D (const UT_DVec3D v, UT_Tfm3D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      rotation, first around the 0, then around the 1
 *                      and then the 2 axis, as specified by v, is generated.
 *                      T and M are then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmConcatRot3D (const UT_DVec3D v, UT_Tfm3D T)
{
    UT_Tfm3D M;
    UT_Vec3D v32;
    v32[0] = v[0];
    v32[1] = v[1];
    v32[2] = v[2];

    UT_TfmBuildRot3D (v32, M);
    UT_TfmConcat3D (T, M, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_DTfmConcatDRot3D
 *
 *      Usage:          Concatenate a 3D transformation matrix with a
 *                      rotation vector.
 *
 *      Synopsis:       void UT_DTfmConcatDRot3D (const UT_DVec3D v, UT_DTfm3D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      rotation, first around the 0, then around the 1
 *                      and then the 2 axis, as specified by v, is generated.
 *                      T and M are then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_DTfmConcatDRot3D (const UT_DVec3D v, UT_DTfm3D T)
{
    UT_DTfm3D M;

    UT_DTfmBuildDRot3D (v, M);
    UT_DTfmConcat3D (T, M, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmConcatScale2D
 *
 *      Usage:          Concatenate a 2D transformation matrix with a
 *                      scaling vector.
 *
 *      Synopsis:       void UT_TfmConcatScale2D (const UT_Vec2D v, UT_Tfm2D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      scaling operation, where v defines the scaling
 *                      factors along the coordinate axes, is generated.
 *                      T and M are then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmConcatScale2D (const UT_Vec2D v, UT_Tfm2D T)
{
    UT_Tfm2D M;

    UT_TfmBuildScale2D (v, M);
    UT_TfmConcat2D (T, M, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmConcatScale3D
 *
 *      Usage:          Concatenate a 3D transformation matrix with a
 *                      scaling vector.
 *
 *      Synopsis:       void UT_TfmConcatScale3D (const UT_Vec3D v, UT_Tfm3D T)
 *
 *      Description:    An intermediate matrix, M, representing a
 *                      scaling operation, where v defines the scaling
 *                      factors along the coordinate axes, is generated.
 *                      T and M are then concatenated (T = TM).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmConcatScale3D (const UT_Vec3D v, UT_Tfm3D T)
{
    UT_Tfm3D M;

    UT_TfmBuildScale3D (v, M);
    UT_TfmConcat3D (T, M, T);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmInvert2D
 *
 *      Usage:          Invert a 2D transformation matrix.
 *
 *      Synopsis:       UT_Bool UT_TfmInvert2D (const UT_Tfm2D A, UT_Tfm2D R)
 *
 *      Description:    The inverse of transformation A is computed and
 *                      stored in R.
 *                      
 *                      We compute the inverse of A using determinants.
 *                      For an explanation of the method, see
 *
 *                              Ilja Bronstein, Konstantin Semendjajev:
 *                              "Taschenbuch der Mathematik",
 *                              Verlag Harry Deutsch, Thun 1985 p. 154
 *
 *      Return value:   UT_True if R could be computed, UT_False if A is singular.
 *
 *      See also:
 *
 ***************************************************************************/

UT_Bool UT_TfmInvert2D (const UT_Tfm2D A, UT_Tfm2D R)
{
    Int32   i, j;
    Float64 fr, mr;

    R[0][0] =  A[1][1];
    R[0][1] = -A[0][1];
    R[1][0] = -A[1][0];
    R[1][1] =  A[0][0];
    fr = A[0][0] * A[1][1] - A[1][0] * A[0][1];
    if (UT_ABS (fr) >= 1.0) {
        for (i = 0; i < 2; ++i) {
            for (j = 0; j < 2; ++j) {
                R[i][j] /= fr;
            }
        }
    } else {
        mr = UT_ABS (fr) / MinNrmFloat32;
        for (i = 0; i < 2; ++i) {
            for (j = 0; j < 2; ++j) {
                if (mr > UT_ABS (R[i][j])) {
                    R[i][j] /= fr;
                } else {
                    return UT_False;
                }
            }
        }
    }
    R[2][0] = -A[2][0] * R[0][0] - A[2][1] * R[1][0];
    R[2][1] = -A[2][0] * R[0][1] - A[2][1] * R[1][1];
    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmInvert3D
 *
 *      Usage:          Invert a 3D transformation matrix.
 *
 *      Synopsis:       UT_Bool UT_TfmInvert3D (const UT_Tfm3D A, UT_Tfm3D R)
 *
 *      Description:    The inverse of transformation A is computed and
 *                      stored in R.
 *                      
 *                      We compute the inverse of A using determinants.
 *                      For an explanation of the method, see
 *
 *                              Ilja Bronstein, Konstantin Semendjajev:
 *                              "Taschenbuch der Mathematik",
 *                              Verlag Harry Deutsch, Thun 1985 p. 154
 *
 *      Return value:   UT_True if R could be computed, UT_False if A is singular.
 *
 *      See also:
 *
 ***************************************************************************/

UT_Bool UT_TfmInvert3D (const UT_Tfm3D A, UT_Tfm3D R)
{
    Int32   i, j;
    Float64 fr, mr;

    R[0][0] = A[1][1] * A[2][2] - A[2][1] * A[1][2];
    R[0][1] = A[2][1] * A[0][2] - A[0][1] * A[2][2];
    R[0][2] = A[0][1] * A[1][2] - A[1][1] * A[0][2];
    R[1][0] = A[2][0] * A[1][2] - A[1][0] * A[2][2];
    R[1][1] = A[0][0] * A[2][2] - A[2][0] * A[0][2];
    R[1][2] = A[1][0] * A[0][2] - A[0][0] * A[1][2];
    R[2][0] = A[1][0] * A[2][1] - A[2][0] * A[1][1];
    R[2][1] = A[2][0] * A[0][1] - A[0][0] * A[2][1];
    R[2][2] = A[0][0] * A[1][1] - A[1][0] * A[0][1];
    fr = A[0][0] * R[0][0] + A[0][1] * R[1][0] + A[0][2] * R[2][0];
    if (UT_ABS (fr) >= 1.0) {
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                R[i][j] /= fr;
            }
        }
    } else {
        mr = UT_ABS (fr) / MinNrmFloat32;
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                if (mr > UT_ABS (R[i][j])) {
                    R[i][j] /= fr;
                } else {
                    return UT_False;
                }
            }
        }
    }
    R[3][0] = -A[3][0] * R[0][0] - A[3][1] * R[1][0] - A[3][2] * R[2][0];
    R[3][1] = -A[3][0] * R[0][1] - A[3][1] * R[1][1] - A[3][2] * R[2][1];
    R[3][2] = -A[3][0] * R[0][2] - A[3][1] * R[1][2] - A[3][2] * R[2][2];
    return UT_True;
}

void UT_TfmTranspose3D (const UT_Tfm3D T, UT_Tfm3D R)
{
    R[0][0] = T[0][0];
    R[0][1] = T[1][0];
    R[0][2] = T[2][0];
    R[1][0] = T[0][1];
    R[1][1] = T[1][1];
    R[1][2] = T[2][1];
    R[2][0] = T[0][2];
    R[2][1] = T[1][2];
    R[2][2] = T[2][2];
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmCoord2D
 *
 *      Usage:          Build a 2D transformation matrix that changes the
 *                      coordinate system.
 *
 *      Synopsis:       void UT_TfmCoord2D
 *                              (const UT_CoSys2D *A, const UT_CoSys2D *B, UT_Tfm2D T)
 *
 *      Description:    A transformation T is built, so that a vector
 *                      representing a point in space using coordinates
 *                      relative to coordinate system a will change into a
 *                      representation of the same point, using coordinates
 *                      relative to coordinate system b, when T is applied.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmCoord2D (const UT_CoSys2D *A, const UT_CoSys2D *B, UT_Tfm2D T)
{
    UT_Vec2D offset;

    UT_VecSub2D (A->off, B->off, offset);
    T[0][0] = UT_VecDotProd2D (A->ori[0], B->ori[0]);
    T[0][1] = UT_VecDotProd2D (A->ori[0], B->ori[1]);
    T[1][0] = UT_VecDotProd2D (A->ori[1], B->ori[0]);
    T[1][1] = UT_VecDotProd2D (A->ori[1], B->ori[1]);
    T[3][0] = UT_VecDotProd2D (B->ori[0], offset);
    T[3][1] = UT_VecDotProd2D (B->ori[1], offset);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmCoord3D
 *
 *      Usage:          Build a 3D transformation matrix that changes the
 *                      coordinate system.
 *
 *      Synopsis:       void UT_TfmCoord3D
 *                              (const UT_CoSys3D *A, const UT_CoSys3D *B, UT_Tfm3D T)
 *
 *      Description:    A transformation T is built, so that a vector
 *                      representing a point in space using coordinates
 *                      relative to coordinate system A will change into a
 *                      representation of the same point, using coordinates
 *                      relative to coordinate system B, when T is applied.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmCoord3D (const UT_CoSys3D *A, const UT_CoSys3D *B, UT_Tfm3D T)
{
    UT_Vec3D offset;

    UT_VecSub3D (A->off, B->off, offset);
    T[0][0] = UT_VecDotProd3D (A->ori[0], B->ori[0]);
    T[0][1] = UT_VecDotProd3D (A->ori[0], B->ori[1]);
    T[0][2] = UT_VecDotProd3D (A->ori[0], B->ori[2]);
    T[1][0] = UT_VecDotProd3D (A->ori[1], B->ori[0]);
    T[1][1] = UT_VecDotProd3D (A->ori[1], B->ori[1]);
    T[1][2] = UT_VecDotProd3D (A->ori[1], B->ori[2]);
    T[2][0] = UT_VecDotProd3D (A->ori[2], B->ori[0]);
    T[2][1] = UT_VecDotProd3D (A->ori[2], B->ori[1]);
    T[2][2] = UT_VecDotProd3D (A->ori[2], B->ori[2]);
    T[3][0] = UT_VecDotProd3D (B->ori[0], offset);
    T[3][1] = UT_VecDotProd3D (B->ori[1], offset);
    T[3][2] = UT_VecDotProd3D (B->ori[2], offset);
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmInitCoord2D
 *
 *      Usage:          Initialize a 2D coordinate system.
 *
 *      Synopsis:       void UT_TfmInitCoord2D (UT_CoSys2D *A)
 *
 *      Description:    A coordinate system A is initialized, so that its origin
 *                      is at (0.0, 0.0, 0.0) and its axes are identical to
 *                      the X (0) and Y (1).
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmInitCoord2D (UT_CoSys2D *A)
{
    A->off[0] = A->off[1] = 0.0;
    A->ori[0][0] = 1.0;
    A->ori[0][1] = 0.0;
    A->ori[1][0] = 0.0;
    A->ori[1][1] = 1.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmInitCoord3D
 *
 *      Usage:          Initialize a 3D coordinate system.
 *
 *      Synopsis:       void UT_TfmInitCoord3D (UT_CoSys3D *A)
 *
 *      Description:    A coordinate system A is initialized, so that its origin
 *                      is at (0.0, 0.0, 0.0) and its axes are identical to
 *                      the X (0), Y (1) and Z (2) axes.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmInitCoord3D (UT_CoSys3D *A)
{
    A->off[0] = A->off[1] = A->off[2] = 0.0;
    A->ori[0][0] = 1.0;
    A->ori[0][1] = 0.0;
    A->ori[0][2] = 0.0;
    A->ori[1][0] = 0.0;
    A->ori[1][1] = 1.0;
    A->ori[1][2] = 0.0;
    A->ori[2][0] = 0.0;
    A->ori[2][1] = 0.0;
    A->ori[2][2] = 1.0;
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmApply2D
 *
 *      Usage:          Apply a 2D transformation matrix to a vector.
 *
 *      Synopsis:       void UT_TfmApply2D
 *                              (const UT_Vec2D v, const UT_Tfm2D T, UT_Vec2D r)
 *
 *      Description:    The point represented by v is moved in space
 *                      according to transformation T (r = vT).
 *                      R must not refer to the same memory location as v.
 *
 *                      There is an identical function "UT_DTfmApply2D" for
 *                      dealing with vectors in double precision.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmApply2D (const UT_Vec2D v, const UT_Tfm2D T, UT_Vec2D r)
{
    r[0] = v[0] * T[0][0] + v[1] * T[1][0] + T[2][0];
    r[1] = v[0] * T[0][1] + v[1] * T[1][1] + T[2][1];
    return;
}

void UT_DTfmApply2D (const UT_DVec2D v, const UT_Tfm2D T, UT_DVec2D r)
{
    r[0] = v[0] * T[0][0] + v[1] * T[1][0] + T[2][0];
    r[1] = v[0] * T[0][1] + v[1] * T[1][1] + T[2][1];
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_TfmApplyNormal3D
 *
 *      Usage:          Apply a 3D transformation to a surface normal.
 *
 *      Synopsis:       void UT_TfmApplyNormal3D
 *                              (const UT_Vec3D n, const UT_Tfm3D T, UT_Vec3D r)
 *
 *      Description:    If n is the normal vector of a plane p in space, vector
 *                      r is determined so that it is the normal of the plane
 *                      that results from transforming all points of p
 *                      according to transformation T.
 *                      R is calculated as follows: Two vectors a and b are
 *                      determined so that they are perpendicular to n and to
 *                      each other. A transformation T', which is T without the
 *                      translational part, is then applied to a and b.
 *                      Finally, r is the vector product of aT' and bT'.
 *                      Note that r is not scaled to length 1.0!
 *                      R may refer to the same memory location as n.
 *
 *                      There is an identical function "UT_DTfmApplyNormal3D"
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_TfmApplyNormal3D (const UT_Vec3D n, const UT_Tfm3D T, UT_Vec3D r)
{
    UT_Vec3D a, b, A, B;

    if (UT_ABS (n[0]) < UT_ABS (n[1]) && UT_ABS (n[0]) < UT_ABS (n[1])) {
        a[0] = 0.0;
        a[1] = n[2];
        a[2] = -n[1];
    } else {
        if (UT_ABS (n[1]) < UT_ABS (n[2])) {
            a[0] = -n[2];
            a[1] = 0.0;
            a[2] = n[0];
        } else {
            a[0] = n[1];
            a[1] = -n[0];
            a[2] = 0.0;
        }
    }
    b[0] = n[1] * a[2] - a[1] * n[2];
    b[1] = n[2] * a[0] - a[2] * n[0];
    b[2] = n[0] * a[1] - a[0] * n[1];
    A[0] = a[0] * T[0][0] + a[1] * T[1][0] + a[2] * T[2][0];
    A[1] = a[0] * T[0][1] + a[1] * T[1][1] + a[2] * T[2][1];
    A[2] = a[0] * T[0][2] + a[1] * T[1][2] + a[2] * T[2][2];
    B[0] = b[0] * T[0][0] + b[1] * T[1][0] + b[2] * T[2][0];
    B[1] = b[0] * T[0][1] + b[1] * T[1][1] + b[2] * T[2][1];
    B[2] = b[0] * T[0][2] + b[1] * T[1][2] + b[2] * T[2][2];
    r[0] = A[1] * B[2] - B[1] * A[2];
    r[1] = A[2] * B[0] - B[2] * A[0];
    r[2] = A[0] * B[1] - B[0] * A[1];
    return;
}

void UT_DTfmApplyNormal3D (const UT_DVec3D n, const UT_Tfm3D T, UT_DVec3D r)
{
    UT_DVec3D a, b, A, B;

    if (UT_ABS (n[0]) < UT_ABS (n[1]) && UT_ABS (n[0]) < UT_ABS (n[1])) {
        a[0] = 0.0;
        a[1] = n[2];
        a[2] = -n[1];
    } else {
        if (UT_ABS (n[1]) < UT_ABS (n[2])) {
            a[0] = -n[2];
            a[1] = 0.0;
            a[2] = n[0];
        } else {
            a[0] = n[1];
            a[1] = -n[0];
            a[2] = 0.0;
        }
    }
    b[0] = n[1] * a[2] - a[1] * n[2];
    b[1] = n[2] * a[0] - a[2] * n[0];
    b[2] = n[0] * a[1] - a[0] * n[1];
    A[0] = a[0] * T[0][0] + a[1] * T[1][0] + a[2] * T[2][0];
    A[1] = a[0] * T[0][1] + a[1] * T[1][1] + a[2] * T[2][1];
    A[2] = a[0] * T[0][2] + a[1] * T[1][2] + a[2] * T[2][2];
    B[0] = b[0] * T[0][0] + b[1] * T[1][0] + b[2] * T[2][0];
    B[1] = b[0] * T[0][1] + b[1] * T[1][1] + b[2] * T[2][1];
    B[2] = b[0] * T[0][2] + b[1] * T[1][2] + b[2] * T[2][2];
    r[0] = A[1] * B[2] - B[1] * A[2];
    r[1] = A[2] * B[0] - B[2] * A[0];
    r[2] = A[0] * B[1] - B[0] * A[1];
    return;
}
