/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

/**
 *
 * @author nbriere
 */
public class cMatrix3x3
{
    double[][] matrix = new double[3][3];
    
    cMatrix3x3()
    {
    }

    cMatrix3x3(cSpring.Point3D a, cSpring.Point3D b)
    {
        v1.x = a.x;
        v1.y = a.y;
        v1.z = a.z;
        v2.x = b.x;
        v2.y = b.y;
        v2.z = b.z;
        
        quat.setRotation(v1, v2);
        
        setRotation(quat);
    }
    
    cMatrix3x3(cVector a, cVector b)
    {
        quat.setRotation(a, b);
        
        setRotation(quat);
    }
    
    void setRotation(cQuat q)
    {
        matrix[0][0] = 1 - 2*(q.y*q.y + q.z*q.z);
        matrix[1][1] = 1 - 2*(q.x*q.x + q.z*q.z);
        matrix[2][2] = 1 - 2*(q.x*q.x + q.y*q.y);
        matrix[0][1] = 2*(q.x*q.y - q.z*q.w);
        matrix[0][2] = 2*(q.x*q.z + q.y*q.w);
        matrix[1][2] = 2*(q.y*q.z - q.x*q.w);
        matrix[1][0] = 2*(q.x*q.y + q.z*q.w);
        matrix[2][0] = 2*(q.x*q.z - q.y*q.w);
        matrix[2][1] = 2*(q.y*q.z + q.x*q.w);
    }

    void Transform(cSpring.Point3D p)
    {
        temp.set(p);
        
        p.x = (float) (matrix[0][0]*temp.x + matrix[0][1]*temp.y + matrix[0][2]*temp.z);
        p.y = (float) (matrix[1][0]*temp.x + matrix[1][1]*temp.y + matrix[1][2]*temp.z);
        p.z = (float) (matrix[2][0]*temp.x + matrix[2][1]*temp.y + matrix[2][2]*temp.z);
    }
    
    void Transform(cVector p)
    {
        temp.set(p);
        
        p.x = matrix[0][0]*temp.x + matrix[0][1]*temp.y + matrix[0][2]*temp.z;
        p.y = matrix[1][0]*temp.x + matrix[1][1]*temp.y + matrix[1][2]*temp.z;
        p.z = matrix[2][0]*temp.x + matrix[2][1]*temp.y + matrix[2][2]*temp.z;
    }
    
    static cSpring.Point3D temp = new cSpring.Point3D();
    static cQuat quat = new cQuat();
    static cVector v1 = new cVector();
    static cVector v2 = new cVector();
}