178 lines
3.7 KiB
C++
178 lines
3.7 KiB
C++
//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)
|
|
|