/*--------------------------------------\
| Disclaimer:
|   This code is certified by me to work, and work well,
|   Since I have no credentials as yet,
|    (perhaps other than this code here)
|   my certification means nothing whatsoever.
|
| Simply stated: USE AT YOUR OWN RISK
|
| You have permision to use this library free
|  free programs free of charge. 
| However for anything commercial, I get 
|  a copy of the product.
\---------------------------------------*/

#include<math.h>
#include"vector.h"
#include"transform.h"

void orthobasisf(_F o1[3], _F o2[3], _F o3[3], _CF e1[3], _CF e2[3])
{
  (void)unit3f(o1, e1);
  (void)unit3f(o2, crossf(o2, crossf(o2, e1, e2), e1));
  (void)crossf(o3, o1, o2);
}

void rev_orthobasisf(_F o1[3], _F o2[3], _F o3[3], _CF e1[3], _CF e3[3])
{
  (void)unit3f(o1, e1);
  (void)unit3f(o3, crossf(o3, e1, crossf(o3, e3, e1)));
  (void)crossf(o2, o3, o1);
}

void orthobasisd(_D o1[3], _D o2[3], _D o3[3], _CD e1[3], _CD e2[3])
{
  (void)unit3d(o1, e1);
  (void)unit3d(o2, crossd(o2, crossd(o2, e1, e2), e1));
  (void)crossd(o3, o1, o2);
}

void rev_orthobasisd(_D o1[3], _D o2[3], _D o3[3], _CD e1[3], _CD e3[3])
{
  (void)unit3d(o1, e1);
  (void)unit3d(o3, crossd(o3, crossd(o3, e3, e1), e3));
  (void)crossd(o2, o3, o1);
}

void matrixVVXpOf(_F m[16], _CF e1[3], _CF e2[3], _CF pos[3])
{
  orthobasisf(&m[0*4], &m[1*4], &m[2*4], e1, e2);
  (void)transpose4x4f(m, m);
  (void)setcolumn4x4f(m, pos, 3, 0, 3);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixVXVpOf(_F m[16], _CF e1[3], _CF e3[3], _CF pos[3])
{
  rev_orthobasisf(&m[0*4], &m[1*4], &m[2*4], e1, e3);
  (void)transpose4x4f(m, m);
  (void)setcolumn4x4f(m, pos, 3, 0, 3);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixXVVpOf(_F m[16], _CF e2[3], _CF e3[3], _CF pos[3])
{
  orthobasisf(&m[1*4], &m[2*4], &m[0*4], e2, e3);
  (void)transpose4x4f(m, m);
  (void)setcolumn4x4f(m, pos, 3, 0, 3);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixVVXpWf(_F m[16], _CF e1[3], _CF e2[3], _CF pos[3])
{
  orthobasisf(&m[0*4], &m[1*4], &m[2*4], e1, e2);
  m[3+0*4] = -dot3f(&m[0*4], pos);
  m[3+1*4] = -dot3f(&m[1*4], pos);
  m[3+2*4] = -dot3f(&m[2*4], pos);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixVXVpWf(_F m[16], _CF e1[3], _CF e3[3], _CF pos[3])
{
  rev_orthobasisf(&m[0*4], &m[1*4], &m[2*4], e1, e3);
  m[3+0*4] = -dot3f(&m[0*4], pos);
  m[3+1*4] = -dot3f(&m[1*4], pos);
  m[3+2*4] = -dot3f(&m[2*4], pos);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixXVVpWf(_F m[16], _CF e2[3], _CF e3[3], _CF pos[3])
{
  orthobasisf(&m[1*4], &m[2*4], &m[0*4], e2, e3);
  m[3+0*4] = -dot3f(&m[0*4], pos);
  m[3+1*4] = -dot3f(&m[1*4], pos);
  m[3+2*4] = -dot3f(&m[2*4], pos);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixVVXpOd(_D m[16], _CD e1[3], _CD e2[3], _CD pos[3])
{
  orthobasisd(&m[0*4], &m[1*4], &m[2*4], e1, e2);
  (void)transpose4x4d(m, m);
  (void)setcolumn4x4d(m, pos, 3, 0, 3);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0;
  m[3+3*4] = 1.0;
}

void matrixVXVpOd(_D m[16], _CD e1[3], _CD e3[3], _CD pos[3])
{
  rev_orthobasisd(&m[0*4], &m[1*4], &m[2*4], e1, e3);
  (void)transpose4x4d(m, m);
  (void)setcolumn4x4d(m, pos, 3, 0, 3);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixXVVpOd(_D m[16], _CD e2[3], _CD e3[3], _CD pos[3])
{
  orthobasisd(&m[1*4], &m[2*4], &m[0*4], e2, e3);
  (void)transpose4x4d(m, m);
  (void)setcolumn4x4d(m, pos, 3, 0, 3);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixVVXpWd(_D m[16], _CD e1[3], _CD e2[3], _CD pos[3])
{
  orthobasisd(&m[0*4], &m[1*4], &m[2*4], e1, e2);
  m[3+0*4] = -dot3d(&m[0*4], pos);
  m[3+1*4] = -dot3d(&m[1*4], pos);
  m[3+2*4] = -dot3d(&m[2*4], pos);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixVXVpWd(_D m[16], _CD e1[3], _CD e3[3], _CD pos[3])
{
  rev_orthobasisd(&m[0*4], &m[1*4], &m[2*4], e1, e3);
  m[3+0*4] = -dot3d(&m[0*4], pos);
  m[3+1*4] = -dot3d(&m[1*4], pos);
  m[3+2*4] = -dot3d(&m[2*4], pos);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}

void matrixXVVpWd(_D m[16], _CD e2[3], _CD e3[3], _CD pos[3])
{
  orthobasisd(&m[1*4], &m[2*4], &m[0*4], e2, e3);
  m[3+0*4] = -dot3d(&m[0*4], pos);
  m[3+1*4] = -dot3d(&m[1*4], pos);
  m[3+2*4] = -dot3d(&m[2*4], pos);
  m[0+3*4] = m[1+3*4] = m[2+3*4] = 0.0f;
  m[3+3*4] = 1.0f;
}