//ROMTEAM StickObjects (Nicolae Suparatu 10/02/98)

// Get the transform parameters (rotation + translation) in order to move one
// triangle over another one, if they are congruent
// Author: Nicolae Suparatu

#include <math.h>
#include "stdafx.h"
#include "edtrot.hpp"



//************************************************************************************
float sqr(float f){ return f*f;}


// Initialize a Matrix
static void InitMatrix(Matrix &m)
{
  for(int i=0; i<3; i++)
    for(int j=0; j<3; j++) m[i][j] = i == j;
}


// Rotate the vector v over the X axis
static void GetXAlignMatrix(Vector &v, Matrix &m)
{
  float b = (float)sqrt(v[0]*v[0] + v[1]*v[1]);
  float a = (float)sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
  if(a < eps)
  {
    InitMatrix(m);
    return;
  }
  m[0][0] = v[0]/a;
  m[0][1] = v[1]/a;
  m[0][2] = v[2]/a;
  m[1][0] = b > eps ? -v[1]/b : 0;
  m[1][1] = b > eps ? v[0]/b : 1;
  m[1][2] = 0;
  m[2][0] = -m[0][2]*m[1][1];
  m[2][1] = m[0][2]*m[1][0];
  m[2][2] = b/a;
}


// Rotate v2 over v1
static void GetXRotateMatrix(Vector &v1, Vector &v2, Matrix &m)
{
  InitMatrix(m);
  float d = v1[1]*v1[1] + v1[2]*v1[2];
  if(d > eps)
  {
    m[1][1] = (v1[1]*v2[1] + v1[2]*v2[2])/d;
    m[1][2] = (v1[1]*v2[2] - v1[2]*v2[1])/d;
    m[2][1] = -m[1][2];
    m[2][2] = m[1][1];
  }
}


// Matrix with vector product
static void VProd(Matrix &m, Vector &v, Vector &dst)
{
  for(int i=0; i<3; i++)
  {
    dst[i] = 0;
    for(int j=0; j<3; j++) dst[i] += m[i][j]*v[j];
  }
}


// Matrix with Matrix product
static void MProd(Matrix &m1, Matrix &m2, Matrix &dst)
{
  for(int i=0; i<3; i++)
    for(int j=0; j<3; j++)
    {
      dst[i][j] = 0;
      for(int k=0; k<3; k++) dst[i][j] += m1[i][k]*m2[k][j];
    }
}


// Transpose Matrix with Matrix product
static void MTProd(Matrix &m1, Matrix &m2, Matrix &dst)
{
  for(int i=0; i<3; i++)
    for(int j=0; j<3; j++)
    {
      dst[i][j] = 0;
      for(int k=0; k<3; k++) dst[i][j] += m1[k][i]*m2[k][j];
    }
}


// Add vectors
void AddVector(Vector &dst, Vector &src)
{
	for(int i=0; i<3; i++) dst[i] += src[i];
}


// Substract vectors
void SubstractVector(Vector &dst, Vector &src)
{
	for(int i=0; i<3; i++) dst[i] -= src[i];
}


static float Distance(Vector &v1, Vector &v2)
{
	return sqr(v1[0]-v2[0]) + sqr(v1[1] - v2[1]) + sqr(v1[2] - v2[2]);
}

// dst = destination triangle (on lines)
// src = source triangle (on lines)
// r = resulting rotation matrix
// t = resulting translation matrix
int GetTransform(Matrix &dst, Matrix &src, Matrix &r, Vector &t)
{
  Vector v1;
  Matrix R1, R2, R3;

// congruence test
	if(fabs(Distance(dst[0], dst[1]) - Distance(src[0], src[1])) > eps ||
	   fabs(Distance(dst[0], dst[2]) - Distance(src[0], src[2])) > eps || 
		 fabs(Distance(dst[1], dst[2]) - Distance(src[1], src[2])) > eps) return 0;

// Origin translation
	SubstractVector(src[1], src[0]);
	SubstractVector(dst[1], dst[0]);
	SubstractVector(src[2], src[0]);
	SubstractVector(dst[2], dst[0]);

// Rotation
  GetXAlignMatrix(dst[1], R1);
  VProd(R1, src[1], v1);
  GetXAlignMatrix(v1, r);
  MProd(r, R1, R2);
  VProd(R1, dst[2], v1);
  VProd(R2, src[2], t);
  GetXRotateMatrix(v1, t, r);
  MProd(r, R2, R3);
  MTProd(R1, R3, r);

// Retranslation
	VProd(r, src[0], v1);
	CopyMemory(t, dst[0], sizeof(t));
	SubstractVector(t, v1);

	AddVector(src[1], src[0]);
	AddVector(dst[1], dst[0]);
	AddVector(src[2], src[0]);
	AddVector(dst[2], dst[0]);
	return 1;
}


void Convert(Matrix &r, Vector &t, Matrix &mo, Vector &vo)
{
	Matrix mtmp;
	Vector vtmp;
	Matrix aaa;
	
	InitMatrix(aaa);
	MTProd(r, aaa, mtmp);
	MProd(mo, mtmp, aaa);
	CopyMemory(mtmp, aaa, sizeof(Matrix));
	CopyMemory(mo, mtmp, sizeof(Matrix));
	VProd(r, vo, vtmp);
	CopyMemory(vo, vtmp, sizeof(Vector));
	AddVector(vo, t);
}

//ENDROMTEAM StickObjects (Nicolae Suparatu)

