Initial commit
This commit is contained in:
237
engines/titanic/star_control/fpose.cpp
Normal file
237
engines/titanic/star_control/fpose.cpp
Normal file
@@ -0,0 +1,237 @@
|
||||
/* ScummVM - Graphic Adventure Engine
|
||||
*
|
||||
* ScummVM is the legal property of its developers, whose names
|
||||
* are too numerous to list here. Please refer to the COPYRIGHT
|
||||
* file distributed with this source distribution.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "titanic/star_control/fpose.h"
|
||||
#include "titanic/star_control/matrix_transform.h"
|
||||
#include "titanic/star_control/matrix_inv.h"
|
||||
|
||||
#include "math/utils.h"
|
||||
|
||||
namespace Titanic {
|
||||
|
||||
// Non-member functions
|
||||
|
||||
void fposeProd(const FPose &a, const FPose &m, FPose &C) {
|
||||
C._row1._x = a._row1._y * m._row2._x + a._row1._z * m._row3._x + a._row1._x * m._row1._x;
|
||||
C._row1._y = a._row1._x * m._row1._y + m._row2._y * a._row1._y + m._row3._y * a._row1._z;
|
||||
C._row1._z = a._row1._x * m._row1._z + a._row1._y * m._row2._z + a._row1._z * m._row3._z;
|
||||
C._row2._x = m._row1._x * a._row2._x + m._row3._x * a._row2._z + m._row2._x * a._row2._y;
|
||||
C._row2._y = m._row3._y * a._row2._z + m._row1._y * a._row2._x + m._row2._y * a._row2._y;
|
||||
C._row2._z = a._row2._z * m._row3._z + a._row2._x * m._row1._z + a._row2._y * m._row2._z;
|
||||
C._row3._x = m._row1._x * a._row3._x + a._row3._z * m._row3._x + a._row3._y * m._row2._x;
|
||||
C._row3._y = a._row3._y * m._row2._y + a._row3._z * m._row3._y + a._row3._x * m._row1._y;
|
||||
C._row3._z = a._row3._x * m._row1._z + a._row3._y * m._row2._z + a._row3._z * m._row3._z;
|
||||
C._vector._x = a._vector._x * m._row1._x + a._vector._y * m._row2._x + a._vector._z * m._row3._x + m._vector._x;
|
||||
C._vector._y = a._vector._x * m._row1._y + a._vector._y * m._row2._y + a._vector._z * m._row3._y + m._vector._y;
|
||||
C._vector._z = a._vector._x * m._row1._z + a._vector._y * m._row2._z + a._vector._z * m._row3._z + m._vector._z;
|
||||
}
|
||||
|
||||
// Member functions
|
||||
|
||||
FPose::FPose() {
|
||||
clear();
|
||||
}
|
||||
|
||||
FPose::FPose(Axis axis, float amount) {
|
||||
setRotationMatrix(axis, amount);
|
||||
}
|
||||
|
||||
FPose::FPose(const FPose &s1, const FPose &s2) {
|
||||
fposeProd(s1, s2, *this);
|
||||
}
|
||||
|
||||
FPose::FPose(int mode, const FVector &src) {
|
||||
switch (mode) {
|
||||
case 0:
|
||||
_row1._x = 1.0;
|
||||
_row2._y = 1.0;
|
||||
_row3._z = 1.0;
|
||||
_vector = src;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
_row1._x = src._x;
|
||||
_row2._y = src._y;
|
||||
_row3._z = src._z;
|
||||
break;
|
||||
|
||||
default:
|
||||
_row1._x = 1.0;
|
||||
_row2._y = 1.0;
|
||||
_row3._z = 1.0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void FPose::identity() {
|
||||
FMatrix::identity();
|
||||
_vector.clear();
|
||||
}
|
||||
|
||||
// Source: https://en.wikipedia.org/wiki/Rotation_matrix
|
||||
void FPose::setRotationMatrix(Axis axis, float amount) {
|
||||
float sinVal = sin(Math::deg2rad<float>(amount));
|
||||
float cosVal = cos(Math::deg2rad<float>(amount));
|
||||
|
||||
switch (axis) {
|
||||
case X_AXIS:
|
||||
_row1._x = 1.0;
|
||||
_row1._y = 0.0;
|
||||
_row1._z = 0.0;
|
||||
_row2._x = 0.0;
|
||||
_row2._y = cosVal;
|
||||
_row2._z = sinVal;
|
||||
_row3._x = 0.0;
|
||||
_row3._y = -sinVal;
|
||||
_row3._z = cosVal;
|
||||
break;
|
||||
|
||||
case Y_AXIS:
|
||||
_row1._x = cosVal;
|
||||
_row1._y = 0.0;
|
||||
_row1._z = -sinVal;
|
||||
_row2._x = 0.0;
|
||||
_row2._y = 1.0;
|
||||
_row2._z = 0.0;
|
||||
_row3._x = sinVal;
|
||||
_row3._y = 0.0;
|
||||
_row3._z = cosVal;
|
||||
break;
|
||||
|
||||
case Z_AXIS:
|
||||
_row1._x = cosVal;
|
||||
_row1._y = sinVal;
|
||||
_row1._z = 0.0;
|
||||
_row2._x = -sinVal;
|
||||
_row2._y = cosVal;
|
||||
_row2._z = 0.0;
|
||||
_row3._x = 0.0;
|
||||
_row3._y = 0.0;
|
||||
_row3._z = 1.0;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
_vector.clear();
|
||||
}
|
||||
|
||||
void FPose::rotVectAxisY(double angleDeg) {
|
||||
_row1.rotVectAxisY(angleDeg);
|
||||
_row2.rotVectAxisY(angleDeg);
|
||||
_row3.rotVectAxisY(angleDeg);
|
||||
_vector.rotVectAxisY(angleDeg);
|
||||
}
|
||||
|
||||
void FPose::copyFrom(const FPose &src) {
|
||||
_row1 = src._row1;
|
||||
_row2 = src._row2;
|
||||
_row3 = src._row3;
|
||||
_vector = src._vector;
|
||||
}
|
||||
|
||||
void FPose::copyFrom(const FMatrix &src) {
|
||||
_row1 = src._row1;
|
||||
_row2 = src._row2;
|
||||
_row3 = src._row3;
|
||||
}
|
||||
|
||||
FPose FPose::inverseTransform() const {
|
||||
FPose matrix_inv;
|
||||
|
||||
matrix_inv._row1._x = _row1._x;
|
||||
matrix_inv._row2._x = _row1._y;
|
||||
matrix_inv._row3._x = _row1._z;
|
||||
matrix_inv._row1._y = _row2._x;
|
||||
matrix_inv._row2._y = _row2._y;
|
||||
matrix_inv._row3._y = _row2._z;
|
||||
matrix_inv._row1._z = _row3._x;
|
||||
matrix_inv._row2._z = _row3._y;
|
||||
matrix_inv._row3._z = _row3._z;
|
||||
|
||||
float A[16]={_row1._x,_row1._y,_row1._z, 0.0,
|
||||
_row2._x,_row2._y,_row2._z, 0.0,
|
||||
_row3._x,_row3._y,_row3._z, 0.0,
|
||||
_vector._x,_vector._y,_vector._z, 1.0};
|
||||
// Inverse matrix
|
||||
float B[16]={};
|
||||
|
||||
// B contains inverse of A
|
||||
matrix4Inverse<float>(A,B);
|
||||
matrix_inv._vector._x=B[12];
|
||||
matrix_inv._vector._y=B[13];
|
||||
matrix_inv._vector._z=B[14];
|
||||
|
||||
return matrix_inv;
|
||||
}
|
||||
|
||||
//TODO: Check math and provide source
|
||||
void FPose::loadTransform(const CMatrixTransform &src) {
|
||||
double total = src.fn1();
|
||||
double factor = (total <= 0.0) ? 0.0 : 2.0 / total;
|
||||
FVector temp1V = src._vector * factor;
|
||||
FVector temp2V = temp1V * src._vector;
|
||||
|
||||
double val1 = temp1V._y * src._vector._x;
|
||||
double val2 = temp1V._z * src._vector._x;
|
||||
double val3 = temp1V._z * src._vector._y;
|
||||
double val4 = temp1V._x * src._field0;
|
||||
double val5 = temp1V._y * src._field0;
|
||||
double val6 = temp1V._z * src._field0;
|
||||
|
||||
_row1._x = 1.0 - (temp2V._z + temp2V._y);
|
||||
_row1._y = val1 + val6;
|
||||
_row1._z = val2 - val5;
|
||||
_row2._x = val1 - val6;
|
||||
_row2._y = 1.0 - (temp2V._z + temp2V._x);
|
||||
_row2._z = val3 + val4;
|
||||
_row3._x = val2 + val5;
|
||||
_row3._y = val3 - val4;
|
||||
_row3._z = 1.0 - (temp2V._y + temp2V._x);
|
||||
_vector._x = 0;
|
||||
_vector._y = 0;
|
||||
_vector._z = 0;
|
||||
}
|
||||
|
||||
FPose FPose::compose(const FMatrix &m) {
|
||||
FPose dm;
|
||||
FPose am;
|
||||
am._row1 = m._row1;
|
||||
am._row2 = m._row2;
|
||||
am._row3 = m._row3;
|
||||
|
||||
fposeProd(*this, am, dm);
|
||||
|
||||
return dm;
|
||||
}
|
||||
|
||||
FPose FPose::compose2(const FPose &m) {
|
||||
FPose dm;
|
||||
dm._row1 = _row1.matProdRowVect(m);
|
||||
dm._row2 = _row2.matProdRowVect(m);
|
||||
dm._row3 = _row3.matProdRowVect(m);
|
||||
dm._vector = _vector.matProdRowVect(m);
|
||||
|
||||
return dm;
|
||||
}
|
||||
|
||||
} // End of namespace Titanic
|
||||
Reference in New Issue
Block a user