/*==========================================================================
 *                                                                         *
 *  Copyright (C) 2001 Dymchenko L.A.                                      *
 *                                                                         *
 *  VirtualRay (Spherical Engine) additional sourse file                   *
 *                                                                         *
 ***************************************************************************/


#include "windows.h"
#include "types@.h"
#include "linalg@.h"
#include "Service_.h"


void rvectort::ortbasis(rvectort& v2, rvectort& v3) const
{
    if (x!=0)
    {
        v2.x=-y;
        v2.y=x;
        v2.z=0;
                
        // added by Peter Popov
        v2.normalize();

        v3=(*this)&v2;

        return;
    }

    if (y!=0)
    {
        v2.x=y;
        v2.y=-x;
        v2.z=0;

        // added by Peter Popov
        v2.normalize();

        v3=v2&(*this);

        return;
    }

    v2.x=z;
    v2.y=0;
    v2.z=-x;

    // added by Peter Popov
    v2.normalize();

    v3=(*this)&v2;
}

void rvectort::ortbasisz(rvectort& v1, rvectort& v2) const
{
    rvectort ortx,negortx,orty,ortz;
    rvectort v;


    ortx=rvectort(1,0,0);
    negortx=rvectort(-1,0,0);
    orty=rvectort(0,1,0);
    ortz=rvectort(0,0,1);
    
  
  if ((equality(*this,ortx,0.0001f)==false)&&(equality(*this,negortx,0.0001f)==false))
    {
        v2=(*this)&ortx;
        
        v2.normalize();

        v1=v2&(*this);

        return;
    }

    v1=orty&(*this);
        
    v2=(*this)&v1;
}


void rvectort::definerandom()
{
  x=2.0f*(rand()&1023)/1024.0f-1.0f;
   y=2.0f*(rand()&1023)/1024.0f-1.0f;
  z=2.0f*(rand()&1023)/1024.0f-1.0f;

    if ((x!=0)||(y!=0)||(z!=0))
        normalize();
}

void rvectort::randomvariation(rnumt intense)
{
    rvectort vx,vy;
    rnumt d;

    ortbasisz(vx,vy);

    d=intense*((rand()&1023)/1024.0f-0.5f);

    vx*=d;
    vy*=d;

    (*this)+=vx;
    (*this)+=vy;

    if ((x!=0)||(y!=0)||(z!=0))
        normalize();
}

//////////////////////////////////////////////////////////////////////////

void rmatrt::normalizecolumns()
{
    rnumt norm;


    norm=m11*m11+m12*m12+m13*m13;
    if (norm>0.000000000001f) 
    {
        norm=1.0f/sqrt(norm);

        m11*=norm;
        m12*=norm;
        m13*=norm;
    }

    norm=m21*m21+m22*m22+m23*m23;
    if (norm>0.000000000001f) 
    {
        norm=1.0f/sqrt(norm);

        m21*=norm;
        m22*=norm;
        m23*=norm;
    }

    norm=m31*m31+m32*m32+m33*m33;
    if (norm>0.000000000001f) 
    {
        norm=1.0f/sqrt(norm);

        m31*=norm;
        m32*=norm;
        m33*=norm;
    }
}


void basisobj::rotatex(rnumt phi)
{
    rnumt cosphi=cos(phi);
    rnumt sinphi=sin(phi);

    rvectort newv2,newv3;


    newv2.x=cosphi*m21+sinphi*m31;
    newv2.y=cosphi*m22+sinphi*m32;
    newv2.z=cosphi*m23+sinphi*m33;

    newv3.x=-sinphi*m21+cosphi*m31;
    newv3.y=-sinphi*m22+cosphi*m32;
    newv3.z=-sinphi*m23+cosphi*m33;


    m21=newv2.x;
    m22=newv2.y;
    m23=newv2.z;

    m31=newv3.x;
    m32=newv3.y;
    m33=newv3.z;
}

void basisobj::rotatey(rnumt phi)
{
    rnumt cosphi=cos(phi);
    rnumt sinphi=sin(phi);

    rvectort newv3,newv1;


    newv3.x=cosphi*m31+sinphi*m11;
    newv3.y=cosphi*m32+sinphi*m12;
    newv3.z=cosphi*m33+sinphi*m13;

    newv1.x=-sinphi*m31+cosphi*m11;
    newv1.y=-sinphi*m32+cosphi*m12;
    newv1.z=-sinphi*m33+cosphi*m13;


    m11=newv1.x;
    m12=newv1.y;
    m13=newv1.z;

    m31=newv3.x;
    m32=newv3.y;
    m33=newv3.z;
}

void basisobj::rotatez(rnumt phi)
{                   
    rnumt cosphi=cos(phi);
    rnumt sinphi=sin(phi);

    rvectort newv1,newv2;


    newv1.x=cosphi*m11+sinphi*m21;
    newv1.y=cosphi*m12+sinphi*m22;
    newv1.z=cosphi*m13+sinphi*m23;

    newv2.x=-sinphi*m11+cosphi*m21;
    newv2.y=-sinphi*m12+cosphi*m22;
    newv2.z=-sinphi*m13+cosphi*m23;


    m11=newv1.x;
    m12=newv1.y;
    m13=newv1.z;

    m21=newv2.x;
    m22=newv2.y;
    m23=newv2.z;
}


void basisobj::setrx(rnumt phi)
{
    rnumt cosphi,sinphi;

    cosphi=cos(phi);
    sinphi=sin(phi);

    m11=1.0f;
    m12=0.0f;
    m13=0.0f;

    m21=0.0f;
    m22=cosphi;
    m23=sinphi;

    m31=0.0f;
    m32=-sinphi;
    m33=cosphi;
}

void basisobj::setry(rnumt phi)
{
    rnumt cosphi,sinphi;

    cosphi=cos(phi);
    sinphi=sin(phi);

    m11=cosphi;
    m12=0.0f;
    m13=-sinphi;

    m21=0.0f;
    m22=1.0f;
    m23=0.0f;

    m31=sinphi;
    m32=0.0f;
    m33=cosphi;
}

void basisobj::setrz(rnumt phi)
{
    rnumt cosphi,sinphi;

    cosphi=cos(phi);
    sinphi=sin(phi);

    m11=cosphi;
    m12=sinphi;
    m13=0.0f;

    m21=-sinphi;
    m22=cosphi;
    m23=0.0f;

    m31=0.0f;
    m32=0.0f;
    m33=1.0f;
}


void basisobj::setdir(const rvectort& dirv, rnumt phi)
{
  rvectort vz,vx,vy;

  vz=v3();
  vz.ortbasisz(vx,vy);
  v1()=vx;
  v2()=vy;
}


rmatrt operator * (const rmatrt& a, const rmatrt& b)
{
    rmatrt m;

    m.m11=a.m11*b.m11+a.m21*b.m12+a.m31*b.m13;
    m.m21=a.m11*b.m21+a.m21*b.m22+a.m31*b.m23;
    m.m31=a.m11*b.m31+a.m21*b.m32+a.m31*b.m33;

    m.m12=a.m12*b.m11+a.m22*b.m12+a.m32*b.m13;
    m.m22=a.m12*b.m21+a.m22*b.m22+a.m32*b.m23;
    m.m32=a.m12*b.m31+a.m22*b.m32+a.m32*b.m33;

    m.m13=a.m13*b.m11+a.m23*b.m12+a.m33*b.m13;
    m.m23=a.m13*b.m21+a.m23*b.m22+a.m33*b.m23;
    m.m33=a.m13*b.m31+a.m23*b.m32+a.m33*b.m33;

    return m;
}

basisobj operator * (const basisobj& a, const basisobj& b)
{
    basisobj m;

    m.m11=a.m11*b.m11+a.m21*b.m12+a.m31*b.m13;
    m.m21=a.m11*b.m21+a.m21*b.m22+a.m31*b.m23;
    m.m31=a.m11*b.m31+a.m21*b.m32+a.m31*b.m33;

    m.m12=a.m12*b.m11+a.m22*b.m12+a.m32*b.m13;
    m.m22=a.m12*b.m21+a.m22*b.m22+a.m32*b.m23;
    m.m32=a.m12*b.m31+a.m22*b.m32+a.m32*b.m33;

    m.m13=a.m13*b.m11+a.m23*b.m12+a.m33*b.m13;
    m.m23=a.m13*b.m21+a.m23*b.m22+a.m33*b.m23;
    m.m33=a.m13*b.m31+a.m23*b.m32+a.m33*b.m33;

    return m;
}


