// Returns a rotation matrix whose third column is equal to the given vector.
Matrix3D Matrix3D::rotationMatrixFromZAxis(const Real3D& dir)
{
// Normalize the target direction
// If it is degenerate, returns the identity matrix
Real3D target = dir;
REAL len = target.mod();
if (len == 0) return Matrix3D(1);
target /= len;
if (!target.isfinite()) return Matrix3D(1);
// Find a non-degenerate vector orthogonal to the target
Real3D v1 = Real3D(1,0,0) ^ target;
REAL l1 = v1.sqrmod();
Real3D v2 = Real3D(0,1,0) ^ target;
REAL l2 = v2.sqrmod();
Real3D v3 = Real3D(0,0,1) ^ target;
REAL l3 = v3.sqrmod();
// Choose the largest among v1,v2,v3
Real3D v;
if (l1 >= l2 && l1 >= l3) v = v1.vers();
else if (l2 >= l1 && l2 >= l3) v = v2.vers();
else /* if (l3 >= l1 && l3 >= l2) */ v = v3.vers();
// Build the rotation matrix
Real3D w = (target^v).vers();
Matrix3D rot;
rot.setCols(v, w, target);
// Debug Check
assert(fabs(rot.det()-1) < Math::TOLERANCE);
return rot;
}
Matrix3D Matrix3D::closestRotation(void) const
{
Matrix3D matrix = (*this);
Matrix3D rt1 = Matrix3D();
Matrix3D rt2 = Matrix3D();
Matrix3D temp = Matrix3D();
matrix.SVD(rt1, temp, rt2);
if (signbit(temp(0,0)))
temp(0, 0) = -1;
else
temp(0, 0) = 1;
if (signbit(temp(1,1)))
temp(1, 1) = -1;
else
temp(1, 1) = 1;
if (signbit(temp(2,2)))
temp(2, 2) = -1;
else
temp(2, 2) = 1;
// lambdas
double l1 = temp(0, 0);
double l2 = temp(1, 1);
double l3 = temp(2, 2);
if (l1*l2*l3 != 1)
temp(2, 2) = -temp(2, 2);
return rt1*temp*rt2;
}
/// Returns a rotation matrix whose third column is equal to the given vector.
Matrix3D Matrix3D::rotationMatrixFromZAxisForDXF(const Real3D& dir)
{
// Normalize the target direction
// If it is degenerate, returns the identity matrix
Real3D target = dir;
REAL len = target.mod();
if (len == 0) return Matrix3D(1);
target /= len;
if (!target.isfinite()) return Matrix3D(1);
// Choice of xAxis: see AutoCAD documentation.
Real3D v;
if( fabs(target[0])*64 < 1 && fabs(target[1])*64 < 1 )
v = Real3D(0,1,0) ^ target;
else
v = Real3D(0,0,1) ^ target;
v = v.vers();
Real3D w = (target^v).vers();
Matrix3D rot;
rot.setCols(v, w, target);
// Debug Check
assert(fabs(rot.det()-1) < Math::TOLERANCE);
return rot;
}
const Matrix3D Matrix3D::MakeRotation(float xRotation, float yRotation, float zRotation)
{
Matrix3D tempX;
Matrix3D tempY;
Matrix3D tempZ;
Matrix3D temp;
tempX = Matrix3D(1,0,0,0,
0,cos(xRotation),sin(xRotation),0,
0,-sin(xRotation),cos(xRotation),0,
0,0,0,1);
tempY = Matrix3D(cos(yRotation),0,-sin(yRotation),0,
0,1,0,0,
sin(yRotation),0,cos(yRotation),0,
0,0,0,1);
tempZ = Matrix3D(cos(zRotation),sin(zRotation),0,0,
-sin(zRotation),cos(zRotation),0,0,
0,0,1,0,
0,0,0,1);
temp = tempX * tempY * tempZ;
return temp;
}
/////////////////////
// Animation stuff //
/////////////////////
Matrix3D::Matrix3D(const Point3D& e)
{
// x, y, z angles
double x = e.p[0];
double y = e.p[1];
double z = e.p[2];
Matrix3D xRotate = Matrix3D();
Matrix3D yRotate = Matrix3D();
Matrix3D zRotate = Matrix3D();
// x-rotations
xRotate(0, 0) = 1;
xRotate(0, 1) = 0;
xRotate(0, 2) = 0;
xRotate(1, 0) = 0;
xRotate(1, 1) = cos(x);
xRotate(1, 2) = sin(x);
xRotate(2, 0) = 0;
xRotate(2, 1) = -sin(x);
xRotate(2, 2) = cos(x);
// y-rotations
yRotate(0, 0) = cos(y);
yRotate(0, 1) = 0;
yRotate(0, 2) = -sin(y);
yRotate(1, 0) = 0;
yRotate(1, 1) = 1;
yRotate(1, 2) = 0;
yRotate(2, 0) = sin(y);
yRotate(2, 1) = 0;
yRotate(2, 2) = cos(y);
// z-rotations
zRotate(0, 0) = cos(z);
zRotate(0, 1) = sin(z);
zRotate(0, 2) = 0;
zRotate(1, 0) = -sin(z);
zRotate(1, 1) = cos(z);
zRotate(1, 2) = 0;
zRotate(2, 0) = 0;
zRotate(2, 1) = 0;
zRotate(2, 2) = 1;
(*this) = zRotate*yRotate*xRotate;
}
Matrix3D::Matrix3D(const Quaternion& q)
{
Quaternion t = q;
t = t.unit();
double k, x, y, z;
k = t.real;
x = t.imag[0];
y = t.imag[1];
z = t.imag[2];
Matrix3D m = Matrix3D();
m(0, 0) = 1 - (2*y*y) - (2*z*z);
m(0, 1) = (2*x*y) + (2*z*k);
m(0, 2) = (2*x*z) - (2*y*k);
m(1, 0) = (2*x*y) - (2*z*k);
m(1, 1) = 1 - (2*x*x) - (2*z*z);
m(1, 2) = (2*y*z) + (2*x*k);
m(2, 0) = (2*x*z) + (2*y*k);
m(2, 1) = (2*y*z) - (2*x*k);
m(2, 2) = 1 - (2*x*x) - (2*y*y);
(*this) = m;
}
void Reader::getcamK(Matrix3D & K, const Vector3D & cam_dir,
const Vector3D & cam_up, const Vector3D & cam_right)
{
double focal = length(cam_dir);
double aspect = length(cam_right);
double angle = 2 * atan(aspect / 2 / focal);
aspect = aspect / length(cam_up);
// height and width
int M = 480, N = 640;
int width = N, height = M;
// pixel size
double psx = 2*focal*tan(0.5*angle)/N ;
double psy = 2*focal*tan(0.5*angle)/aspect/M ;
psx = psx / focal;
psy = psy / focal;
double Ox = (width+1)*0.5;
double Oy = (height+1)*0.5;
K = Matrix3D( 1.f/psx, 0.f, Ox,
0.f, -1.f/psy, Oy,
0.f, 0.f, 1.f);
}
const Matrix3D Matrix3D::operator*(const Matrix3D &other) const
{
Matrix3D _temp;
_temp = Matrix3D();
/*
Doing the multiplications here.
*/
_temp.SetValues(0,0, ((this->_m[0][0] * other._m[0][0]) + (this->_m[0][1] * other._m[1][0]) + (this->_m[0][2] * other._m[2][0]) + (this->_m[0][3] * this->_m[3][0]))); // 0 - 0
_temp.SetValues(0,1, ((this->_m[0][0] * other._m[0][1]) + (this->_m[0][1] * other._m[1][1]) + (this->_m[0][2] * other._m[2][1]) + (this->_m[0][3] * this->_m[3][1]))); // 0 - 1
_temp.SetValues(0,2, ((this->_m[0][0] * other._m[0][2]) + (this->_m[0][1] * other._m[1][2]) + (this->_m[0][2] * other._m[2][2]) + (this->_m[0][3] * this->_m[3][2]))); // 0 - 2
_temp.SetValues(0,3, ((this->_m[0][0] * other._m[0][3]) + (this->_m[0][1] * other._m[1][3]) + (this->_m[0][2] * other._m[2][3]) + (this->_m[0][3] * this->_m[3][3]))); // 0 - 2
_temp.SetValues(1,0, ((this->_m[1][0] * other._m[0][0]) + (this->_m[1][1] * other._m[1][0]) + (this->_m[1][2] * other._m[2][0]) + (this->_m[1][3] * this->_m[3][0]))); // 1 - 0
_temp.SetValues(1,1, ((this->_m[1][0] * other._m[0][1]) + (this->_m[1][1] * other._m[1][1]) + (this->_m[1][2] * other._m[2][1]) + (this->_m[1][3] * this->_m[3][1]))); // 1 - 1
_temp.SetValues(1,2, ((this->_m[1][0] * other._m[0][2]) + (this->_m[1][1] * other._m[1][2]) + (this->_m[1][2] * other._m[2][2]) + (this->_m[1][3] * this->_m[3][2]))); // 1 - 2
_temp.SetValues(1,3, ((this->_m[1][0] * other._m[0][3]) + (this->_m[1][1] * other._m[1][3]) + (this->_m[1][2] * other._m[2][3]) + (this->_m[1][3] * this->_m[3][3]))); // 1 - 3
_temp.SetValues(2,0, ((this->_m[2][0] * other._m[0][0]) + (this->_m[2][1] * other._m[1][0]) + (this->_m[2][2] * other._m[2][0]) + (this->_m[2][3] * this->_m[3][0]))); // 2 - 0
_temp.SetValues(2,1, ((this->_m[2][0] * other._m[0][1]) + (this->_m[2][1] * other._m[1][1]) + (this->_m[2][2] * other._m[2][1]) + (this->_m[2][3] * this->_m[3][1]))); // 2 - 1
_temp.SetValues(2,2, ((this->_m[2][0] * other._m[0][2]) + (this->_m[2][1] * other._m[1][2]) + (this->_m[2][2] * other._m[2][2]) + (this->_m[2][3] * this->_m[3][2]))); // 2 - 2
_temp.SetValues(2,3, ((this->_m[2][0] * other._m[0][3]) + (this->_m[2][1] * other._m[1][3]) + (this->_m[2][2] * other._m[2][3]) + (this->_m[2][3] * this->_m[3][3]))); // 2 - 3
_temp.SetValues(3,0, ((this->_m[3][0] * other._m[0][1]) + (this->_m[3][1] * other._m[1][0]) + (this->_m[3][2] * other._m[2][0]) + (this->_m[3][3] * this->_m[3][0]))); // 3 - 0
_temp.SetValues(3,1, ((this->_m[3][0] * other._m[0][1]) + (this->_m[3][1] * other._m[1][1]) + (this->_m[3][2] * other._m[2][1]) + (this->_m[3][3] * this->_m[3][1]))); // 3 - 1
_temp.SetValues(3,2, ((this->_m[3][0] * other._m[0][2]) + (this->_m[3][1] * other._m[1][2]) + (this->_m[3][2] * other._m[2][2]) + (this->_m[3][3] * this->_m[3][2]))); // 3 - 2
_temp.SetValues(3,3, ((this->_m[3][0] * other._m[0][2]) + (this->_m[3][1] * other._m[1][3]) + (this->_m[3][2] * other._m[2][3]) + (this->_m[3][3] * this->_m[3][3]))); // 3 - 3
return _temp;
}
int MBSObjectFactory::AddGeomElement(int objectTypeId)
{
// the order of this list MUST coincide with the order in MBSObjectFactory()
switch(objectTypeId) //element type
{
case 1: //GeomMesh3D
{
GeomMesh3D ge(GetMBS(), 0, defaultbodycol);
return GetMBS()->Add(ge);
}
case 2: //GeomCyl3D
{
GeomZyl3D ge(GetMBS(), Vector3D(0.,0.,0.), Vector3D(0.,0.,0.), 0, defaultgeomtile, defaultbodycol);
return GetMBS()->Add(ge);
}
case 3: //GeomSphere3D
{
GeomSphere3D ge(GetMBS(), 0, Vector3D(0.,0.,0.), 0, defaultgeomtile, defaultbodycol);
ge.SetDrawParam(Vector3D(0.001,16,0.));
return GetMBS()->Add(ge);
}
case 4: //GeomCube3D
{
GeomCube3D ge(GetMBS(), Vector3D(0.,0.,0.), Vector3D(1.,1.,1.), defaultbodycol);
return GetMBS()->Add(ge);
}
case 5: //GeomOrthoCube3D
{
GeomOrthoCube3D ge(GetMBS(), Vector3D(0.,0.,0.), Vector3D(1.,1.,1.), Matrix3D(1.), defaultbodycol);
return GetMBS()->Add(ge);
}
}
assert(0);
return -1;
}
void XWing::ReadyForGame()
{
matrix = Matrix3D();
matrix = rotate(DegreesToRadians(180));
scalar = 0.5;
position = Vector3D(800, 450);
rotation = 0;
hasAstromechDroid = true;
}
//this function assigns default values to the element variables
void Mass1D::ElementDefaultConstructorInitialization()
{
type = TBody;
drawres = 8;
x_init = Vector(2*SOS()); //zero initialized
elementname = GetElementSpec();
radius = 0.1; // DR just some default value
pref3D = Vector3D(0.);
rotref3D = Matrix3D(1.);
}
Matrix4D ParametrizedEulerAnglesAndTranslation::getMatrix(void)
{
Point3D angles = value->eulerAngles;
Point3D trans = value->translate;
Matrix3D rotate = Matrix3D(angles);
return Matrix4D(rotate, trans);
}
Matrix4D ParametrizedQuaternionAndTranslation::getMatrix(void)
{
Quaternion q = value->quaternion;
Point3D trans = value->translate;
Matrix3D closestRot = Matrix3D(q);
return Matrix4D(closestRot, trans);
}
Matrix3D
GalacticToEquatorialMatrix( )
{
// This is equivalent to
// Matrix( 2, 270 - 192.25deg ) * Matrix3D( 0, 27.4 - 90deg)
// * Matrix3D( 2, 33deg ).
return Matrix3D( -0.06698873941515, 0.49272846607532, -0.86760081115143,
-0.87275576585199, -0.45034695801996, -0.18837460172292,
-0.48353891463218, 0.74458463328303, 0.46019978478385 );
}
Matrix3D KITTIReader::List2Matrix(const QStringList & n)
{
return Matrix3D(n.at(0).trimmed().toFloat(),
n.at(1).trimmed().toFloat(),
n.at(2).trimmed().toFloat(),
n.at(3).trimmed().toFloat(),
n.at(4).trimmed().toFloat(),
n.at(5).trimmed().toFloat(),
n.at(6).trimmed().toFloat(),
n.at(7).trimmed().toFloat(),
n.at(8).trimmed().toFloat());
}
/* While these Exp and Log implementations are the direct implementations of the Taylor series, the Log
* function tends to run into convergence issues so we use the other ones:*/
Matrix3D Matrix3D::Exp(const Matrix3D& m,int iter)
{
Matrix3D tempx = m;
// factorial
double fact[iter];
Matrix3D dynamicX[iter];
Matrix3D taylor[iter];
fact[0] = 1;
dynamicX[0] = Matrix3D();
taylor[0] = Matrix3D();
for (int i = 1; i < iter; i++)
{
fact[i] = fact[i-1]*i;
dynamicX[i] = tempx*dynamicX[i-1];
taylor[i] = (dynamicX[i]*(1/fact[i])) + taylor[i-1];
}
return taylor[iter-1];
}
// @brief : カメラ行列の作成
//--------------------------------------------------------------------
Matrix3D Camera::Matrix( void )const
{
const Vector3 at = At;
const Vector3 eye = Eye;
const Vector3 up = Up;
#if 0
Vector3 forward;
forward = at- eye;
forward.Normalize();
Vector3 side = forward.Cross(up);
side.Normalize();
Vector3 uper = side.Cross(forward);
uper.Normalize();
return Matrix3D(
side.x,uper.x,-forward.x,0,
side.y,uper.y,-forward.y,0,
side.z,uper.z,-forward.z,0,
0,0,0,1
);
#else
const Vector3 z(Vector3(eye-at).Normalize());
const Vector3 x(up.Cross(z).Normalize());
const Vector3 y(z.Cross(x).Normalize());
return Matrix3D (
x.x, x.y, x.z, -x.Dot(eye),
y.x, y.y, y.z, -y.Dot(eye),
z.x, z.y, z.z, -z.Dot(eye),
0.0f,0.0f,0.0f, 1.0f
);
#endif
}
Matrix3D
HorizontalToEquatorialMatrix( Angle localSiderealTime,
Angle geographicLatitude )
{
// This is equivalent to
// Matrix3D( 2, -lst ) * Matrix3D( 1,0,0, 0,-1,0, 0,0,1 )
// * Matrix3D( 1, lat - pi/2 ) * Matrix3D( 2, pi ).
double sinLST = localSiderealTime.Sin( );
double cosLST = localSiderealTime.Cos( );
double sinLat = geographicLatitude.Sin( );
double cosLat = geographicLatitude.Cos( );
return Matrix3D( - cosLST * sinLat, - sinLST, cosLST * cosLat,
- sinLST * sinLat, cosLST, sinLST * cosLat,
cosLat, 0, sinLat );
}
void Reader::computeRT(Matrix3D & R, Vector3D & t, const Vector3D & cam_dir,
const Vector3D & cam_pos, const Vector3D & cam_up)
{
Vector3D x, y, z;
z = cam_dir / length(cam_dir);
x = cross(cam_up, z);
x = normalize(x);
y = cross(z, x);
R = Matrix3D(x, y, z);
R = R.trans();
t = cam_pos;
}
Transformation3D KITTIReader::List2Transform(const QStringList & n)
{
Transformation3D t;
t.R = Matrix3D(n.at(0).trimmed().toFloat(),
n.at(1).trimmed().toFloat(),
n.at(2).trimmed().toFloat(),
n.at(4).trimmed().toFloat(),
n.at(5).trimmed().toFloat(),
n.at(6).trimmed().toFloat(),
n.at(8).trimmed().toFloat(),
n.at(9).trimmed().toFloat(),
n.at(10).trimmed().toFloat());
t.T = Vector3D(n.at(3).trimmed().toFloat(),
n.at(7).trimmed().toFloat(),
n.at(11).trimmed().toFloat());
return t;
}
Matrix3D
Matrix3D::Adjoint() const
{
return Matrix3D( MINOR(*this, 1, 2, 3, 1, 2, 3),
-MINOR(*this, 0, 2, 3, 1, 2, 3),
MINOR(*this, 0, 1, 3, 1, 2, 3),
-MINOR(*this, 0, 1, 2, 1, 2, 3),
-MINOR(*this, 1, 2, 3, 0, 2, 3),
MINOR(*this, 0, 2, 3, 0, 2, 3),
-MINOR(*this, 0, 1, 3, 0, 2, 3),
MINOR(*this, 0, 1, 2, 0, 2, 3),
MINOR(*this, 1, 2, 3, 0, 1, 3),
-MINOR(*this, 0, 2, 3, 0, 1, 3),
MINOR(*this, 0, 1, 3, 0, 1, 3),
-MINOR(*this, 0, 1, 2, 0, 1, 3),
-MINOR(*this, 1, 2, 3, 0, 1, 2),
MINOR(*this, 0, 2, 3, 0, 1, 2),
-MINOR(*this, 0, 1, 3, 0, 1, 2),
MINOR(*this, 0, 1, 2, 0, 1, 2));
}
void XWing::Initialize()
{
isSpawned = false;
isDead = false;
isPlayer = false;
contactRadius = 82.0f;
health = 100.0f;
totalHealth = health;
armor = 0.8f;
mass = 0.05f;
lifetime = 10.0f;
rotation = 0;
scalar = 1;
acceleration = 40.0f;
turnSpeed = 0.25f;
entityName = "X-Wing";
color.r = 255;
color.g = 255;
color.b = 255;
hb.Initialize();
direction = Vector3D(0, 1);
position = Vector3D();
velocity = Vector3D();
matrix = Matrix3D();
type = XW;
mode = ALT;
engines = new Vector3D[2];
barrels = new Vector3D[2];
barrels[0] = Vector3D(78, 34);
barrels[1] = Vector3D(-78, 34);
currentBarrel = 0;
barrelCount = 2;
bombLastFrame = false;
cannonLastFrame = false;
lazerLastFrame = false;
missleLastFrame = false;
torpedoLastFrame = false;
bombCooldown = 0;
cannonCooldown = 0;
lazerCooldown = 0;
missleCooldown = 0;
torpedoCooldown = 0;
startBombCooldown = false;
startCannonCooldown = false;
startLazerCooldown = false;
startMissleCooldown = false;
startTorpedoCooldown = false;
bombFrequency = 3.0f;
cannonFrequency = 0.5f;
lazerFrequency = 0.1f;
missleFrequency = 1.0f;
torpedoFrequency = 2.0f;
shieldRadius = 1.0f;
hasAstromechDroid = true;
}
//
// Matrix3D.cpp
// TestOpenglES
//
// Created by Neil on 26/8/14.
// Copyright (c) 2014 neil. All rights reserved.
//
#include "core/geom/Matrix3D.h"
#include "core/geom/mmath.h"
#include "platform/PlatformMacros.h"
NS_MONKEY_BEGIN
Matrix3D Matrix3D::_mt = Matrix3D();
void Matrix3D::identity() {
rawData[0] = 1.0f;
rawData[1] = 0.0f;
rawData[2] = 0.0f;
rawData[3] = 0.0f;
rawData[4] = 0.0f;
rawData[5] = 1.0f;
rawData[6] = 0.0f;
rawData[7] = 0.0f;
rawData[8] = 0.0f;
rawData[9] = 0.0f;
rawData[10] = 1.0f;
Matrix3D::Matrix3D(const Quaternion& q){
(*this)=Matrix3D();
}
/////////////////////
// Animation stuff //
/////////////////////
Matrix3D::Matrix3D(const Point3D& e){
(*this)=Matrix3D();
}
bool Reader::Read_FromSource(QImage &ref, Matrix3D &Rref, Vector3D &tref,
QVector<QImage> &src, QVector<Matrix3D> &Rsrc, QVector<Vector3D> tsrc,
Matrix3D &K)
{
// Load 0th image from source directory
QString loc = "/PlaneSweep/im";
QString refr = SOURCE_DIR;
refr += loc;
refr += QString::number(0);
refr += ".png";
if (!ref.load(refr)) return false;
// All required camera matrices (found in 'calibration.txt')
K = Matrix3D({
{0.709874*640, (1-0.977786)*640, 0.493648*640},
{0, 0.945744*480, 0.514782*480},
{0, 0, 1}
});
src.resize(9);
Rsrc.resize(9);
tsrc.resize(9);
Rref = Matrix3D( 0.993701, 0.110304, -0.0197854,
0.0815973, -0.833193, -0.546929,
-0.0768135, 0.541869, -0.836945);
tref = Vector3D( 0.280643, -0.255355, 0.810979);
Rsrc[0] = Matrix3D( 0.993479, 0.112002, -0.0213286,
0.0822353, -0.83349, -0.54638,
-0.0789729, 0.541063, -0.837266);
tsrc[0] = Vector3D( 0.287891, -0.255839, 0.808608);
Rsrc[1] = Matrix3D( 0.993199, 0.114383, -0.0217434,
0.0840021, -0.833274, -0.546442,
-0.0806218, 0.540899, -0.837215);
tsrc[1] = Vector3D( 0.295475, -0.25538, 0.805906);
Rsrc[2] = Matrix3D( 0.992928, 0.116793, -0.0213061,
0.086304, -0.833328, -0.546001,
-0.081524, 0.5403, -0.837514);
tsrc[2] = Vector3D( 0.301659, -0.254563, 0.804653);
Rsrc[3] = Matrix3D( 0.992643, 0.119107, -0.0217442,
0.0880017, -0.833101, -0.546075,
-0.0831565, 0.540144, -0.837454);
tsrc[3] = Vector3D( 0.309666, -0.254134, 0.802222);
Rsrc[4] = Matrix3D( 0.992429, 0.121049, -0.0208028,
0.0901911, -0.833197, -0.545571,
-0.0833736, 0.539564, -0.837806);
tsrc[4] = Vector3D( 0.314892, -0.253009, 0.801559);
Rsrc[5] = Matrix3D( 0.992226, 0.122575, -0.0215154,
0.0911582, -0.833552, -0.544869,
-0.0847215, 0.538672, -0.838245);
tsrc[5] = Vector3D( 0.32067, -0.254142, 0.799812);
Rsrc[6] = Matrix3D( 0.992003, 0.124427, -0.0211509,
0.0930933, -0.834508, -0.543074,
-0.0852237, 0.536762, -0.839418);
tsrc[6] = Vector3D( 0.325942, -0.254865, 0.799037);
Rsrc[7] = Matrix3D( 0.991867, 0.125492, -0.021234,
0.0938678, -0.833933, -0.543824,
-0.0859533, 0.537408, -0.838931);
tsrc[7] = Vector3D( 0.332029, -0.252767, 0.797979);
Rsrc[8] = Matrix3D( 0.991515, 0.128087, -0.0221943,
0.095507, -0.833589, -0.544067,
-0.0881887, 0.53733, -0.838748);
tsrc[8] = Vector3D( 0.33934, -0.250995, 0.796756);
// load source images
QString source;
for (int i = 0; i < 9; i++){
source = SOURCE_DIR;
source += loc;
source += QString::number(i + 1);
source += ".png";
if (!src[i].load(source)) return false;
}
return true;
}
bool AddQualityMetric::evaluate_with_Hessian( PatchData& pd,
size_t handle,
double& value,
std::vector<size_t>& indices,
std::vector<Vector3D>& gradient,
std::vector<Matrix3D>& Hessian,
MsqError& err )
{
std::vector<size_t>::iterator i;
size_t j, r, c, n, h;
double val1, val2;
bool rval1, rval2;
rval1 = metric1.evaluate_with_Hessian( pd, handle, val1, indices1, grad1, Hess1, err ); MSQ_ERRZERO(err);
rval2 = metric2.evaluate_with_Hessian( pd, handle, val2, indices2, grad2, Hess2, err ); MSQ_ERRZERO(err);
indices.resize( indices1.size() + indices2.size() );
i = std::copy( indices1.begin(), indices1.end(), indices.begin() );
std::copy( indices2.begin(), indices2.end(), i );
std::sort( indices.begin(), indices.end() );
indices.erase( std::unique( indices.begin(), indices.end() ), indices.end() );
gradient.clear();
gradient.resize( indices.size(), Vector3D(0.0) );
for (j = 0; j < indices1.size(); ++j)
{
i = std::lower_bound( indices.begin(), indices.end(), indices1[j] );
indices1[j] = i - indices.begin();
gradient[indices1[j]] += grad1[j];
}
for (j = 0; j < indices2.size(); ++j)
{
i = std::lower_bound( indices.begin(), indices.end(), indices2[j] );
indices2[j] = i - indices.begin();
gradient[indices2[j]] += grad2[j];
}
const size_t N = indices.size();
Hessian.clear();
Hessian.resize( N * (N+1) / 2, Matrix3D(0.0) );
n = indices1.size();
h = 0;
for (r = 0; r < n; ++r)
{
const size_t nr = indices1[r];
for (c = r; c < n; ++c)
{
const size_t nc = indices1[c];
if (nr <= nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += Hess1[h++];
else
Hessian[N*nc - nc*(nc+1)/2 + nr].plus_transpose_equal( Hess1[h++] );
}
}
n = indices2.size();
h = 0;
for (r = 0; r < n; ++r)
{
const size_t nr = indices2[r];
for (c = r; c < n; ++c)
{
const size_t nc = indices2[c];
if (nr <= nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += Hess2[h++];
else
Hessian[N*nc - nc*(nc+1)/2 + nr].plus_transpose_equal( Hess2[h++] );
}
}
value = val1 + val2;
return rval1 && rval2;
}
bool ElasticCable::evalInt (LocalIntegral& elmInt,
const FiniteElement& fe,
const Vec3& X) const
{
size_t a, aa, b, bb;
unsigned char i, j, k, l, o;
const size_t nen = fe.N.size();
// Set up reference configuration
Vec3 dX(fe.G.getColumn(1));
Vec3 ddX(fe.G.getColumn(2));
#if INT_DEBUG > 1
std::cout <<"ElasticCable: X = "<< X <<" dX = "<< dX <<" ddX = "<< ddX <<"\n";
#endif
// Compute current configuration
ElmMats& elMat = static_cast<ElmMats&>(elmInt);
const Vector& eV = elMat.vec.front(); // Current displacement vector
Vec3 x(X);
Vec3 dx(dX);
Vec3 ddx(ddX);
for (i = 0; i < 3; i++)
{
x[i] += eV.dot(fe.N,i,3);
dx[i] += eV.dot(fe.dNdX,i,3);
ddx[i] += eV.dot(fe.d2NdX2,i,3);
}
#if INT_DEBUG > 1
std::cout <<"ElasticCable: x = "<< x <<" dx = "<< dx <<" ddx = "<< ddx <<"\n";
#endif
// Compute local coordinate systems of the reference and current configuration
Vec3 B_unit, N_unit;
double B_len, N_len;
if (!evalLocalAxes(dX,ddX,B_unit,N_unit,B_len,N_len)) return false;
#if INT_DEBUG > 1
std::cout <<"ElasticCable: B_unit = "<< B_unit <<" N_unit = "<< N_unit <<"\n";
#endif
Vec3 b_unit, n_unit;
double b_len, n_len;
if (!evalLocalAxes(dx,ddx,b_unit,n_unit,b_len,n_len)) return false;
Vec3 bin = b_unit * b_len;
double b_len2 = b_len * b_len;
Vec3 n = n_unit * n_len;
double n_len2 = n_len * n_len;
#if INT_DEBUG > 1
std::cout <<"ElasticCable: b = "<< bin <<" b_unit = "<< b_unit
<<"\n n = "<< n <<" n_unit = "<< n_unit << std::endl;
#endif
// Calculate derivative of b_unit
std::vector<Matrix> db(nen,Matrix(3,3)), db_unit(nen,Matrix(3,3));
std::vector<Vec3> db_normal(nen);
for (i = 1; i <= 3; i++)
for (k = 1; k <= 3; k++)
for (l = 1; l <= 3; l++)
{
double eps_kli = 0.5*(k-l)*(l-i)*(i-k);
double eps_kil = 0.5*(k-i)*(i-l)*(l-k);
for (a = 1; a <= nen; a++)
db[a-1](k,i) += (eps_kil*fe.dNdX(a,1)*ddx[l-1] +
eps_kli*dx[l-1]*fe.d2NdX2(a,1,1));
}
for (i = 1; i <= 3; i++)
for (a = 0; a < nen; a++)
for (k = 1; k <= 3; k++)
db_normal[a][i-1] += b_unit[k-1]*db[a](k,i);
for (i = 1; i <= 3; i++)
for (k = 1; k <= 3; k++)
for (a = 0; a < nen; a++)
db_unit[a](k,i) += (db[a](k,i) - b_unit[k-1]*db_normal[a][i-1])/b_len;
#if INT_DEBUG > 2
std::cout <<"ElasticCable: db_unit:\n";
for (a = 0; a < nen; a++)
std::cout <<"node "<< a+1 << db_unit[a];
#endif
// Calculate second derivative of b_unit
std::vector< std::vector<Matrix3D> > ddb(nen), ddb_unit(nen);
std::vector< std::vector<Matrix> > ddb_normal(nen);
for (a = 0; a < nen; a++)
{
ddb[a].resize(nen,Matrix3D(3,3,3));
ddb_unit[a].resize(nen,Matrix3D(3,3,3));
ddb_normal[a].resize(nen,Matrix(3,3));
}
for (i = 1; i <= 3; i++)
for (j = 1; j <= 3; j++)
for (k = 1; k <= 3; k++)
{
double eps_kij = 0.5*(k-i)*(i-j)*(j-k);
double eps_kji = 0.5*(k-j)*(j-i)*(i-k);
for (a = 1; a <= nen; a++)
for (b = 1; b <= nen; b++)
ddb[a-1][b-1](k,i,j) = (eps_kji*fe.d2NdX2(a,1,1)*fe.dNdX(b,1) +
eps_kij*fe.d2NdX2(b,1,1)*fe.dNdX(a,1));
}
#if INT_DEBUG > 3
std::cout <<"ElasticCable: ddb:\n";
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
std::cout <<"nodes "<< a+1 <<","<< b+1 << ddb[a][b];
#endif
for (i = 1; i <= 3; i++)
for (j = 1; j <= 3; j++)
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
for (k = 1; k <= 3; k++)
ddb_normal[a][b](i,j) += (ddb[a][b](k,i,j)*bin[k-1] +
db[a](k,i)*db[b](k,j) -
bin[k-1]*db[a](k,i)*bin[k-1]*db[b](k,j) /
b_len2) / b_len;
#if INT_DEBUG > 3
std::cout <<"ElasticCable: ddb_normal:\n";
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
std::cout <<"nodes "<< a+1 <<","<< b+1 << ddb_normal[a][b];
#endif
for (i = 1; i <= 3; i++)
for (j = 1; j <= 3; j++)
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
for (k = 1; k <= 3; k++)
ddb_unit[a][b](k,i,j) = (ddb[a][b](k,i,j)/b_len -
db[a](k,i)*db_normal[b][j-1]/b_len2 -
db[b](k,j)*db_normal[a][i-1]/b_len2 -
bin[k-1]*(ddb_normal[a][b](i,j) -
db_normal[a][i-1]*
db_normal[b][j-1]*2.0 /
b_len) / b_len2);
#if INT_DEBUG > 2
std::cout <<"ElasticCable: ddb_unit:\n";
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
std::cout <<"nodes "<< a+1 <<","<< b+1 << ddb_unit[a][b];
#endif
// Calculate derivative of n_unit
std::vector<Matrix> dn(nen,Matrix(3,3)), dn_unit(nen,Matrix(3,3));
std::vector<Vec3> dn_normal(nen);
for (i = 1; i <= 3; i++)
for (k = 1; k <= 3; k++)
for (l = 1; l <= 3; l++)
{
double eps_kli = 0.5*(k-l)*(l-i)*(i-k);
for (a = 0; a < nen; a++)
{
dn[a](k,i) += eps_kli*b_unit[l-1]*fe.dNdX(1+a,1);
for (o = 1; o <= 3; o++)
{
double eps_kol = 0.5*(k-o)*(o-l)*(l-k);
dn[a](k,i) += eps_kol*db_unit[a](o,i)*dx[l-1];
}
}
}
for (i = 1; i <= 3; i++)
for (a = 0; a < nen; a++)
for (k = 1; k <= 3; k++)
dn_normal[a][i-1] += n_unit[k-1]*dn[a](k,i);
for (i = 1; i <= 3; i++)
for (k = 1; k <= 3; k++)
for (a = 0; a < nen; a++)
dn_unit[a](k,i) += (dn[a](k,i) - n_unit[k-1]*dn_normal[a][i-1])/n_len;
#if INT_DEBUG > 2
std::cout <<"\nElasticCable: dn_unit:\n";
for (a = 0; a < nen; a++)
std::cout <<"node "<< a+1 << dn_unit[a];
#endif
// Calculate second derivative of n_unit
std::vector< std::vector<Matrix3D> > ddn(nen), ddn_unit(nen);
std::vector< std::vector<Matrix> > ddn_normal(nen);
for (a = 0; a < nen; a++)
{
ddn[a].resize(nen,Matrix3D(3,3,3));
ddn_unit[a].resize(nen,Matrix3D(3,3,3));
ddn_normal[a].resize(nen,Matrix(3,3));
}
for (i = 1; i <= 3; i++)
for (j = 1; j <= 3; j++)
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
for (k = 1; k <= 3; k++)
for (o = 1; o <= 3; o++)
{
double eps_koj = 0.5*(k-o)*(o-j)*(j-k);
double eps_koi = 0.5*(k-o)*(o-i)*(i-k);
ddn[a][b](k,i,j) += (eps_koj*db_unit[a](o,i)*fe.dNdX(1+b,1) +
eps_koi*db_unit[b](o,j)*fe.dNdX(1+a,1));
for (l = 1; l <= 3; l++)
{
double eps_kol = 0.5*(k-o)*(o-l)*(l-k);
ddn[a][b](k,i,j) += eps_kol*ddb_unit[a][b](o,i,j)*dx[l-1];
}
}
for (i = 1; i <= 3; i++)
for (j = 1; j <= 3; j++)
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
for (k = 1; k <= 3; k++)
ddn_normal[a][b](i,j) += (ddn[a][b](k,i,j)*n[k-1] +
dn[a](k,i)*dn[b](k,j) -
n[k-1]*dn[a](k,i)*
n[k-1]*dn[b](k,j)/n_len2) / n_len;
for (i = 1; i <= 3; i++)
for (j = 1; j <= 3; j++)
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
for (k = 1; k <= 3; k++)
ddn_unit[a][b](k,i,j) = (ddn[a][b](k,i,j)/n_len -
dn[a](k,i)*dn_normal[b][j-1]/n_len2 -
dn[b](k,j)*dn_normal[a][i-1]/n_len2 -
n[k-1]*(ddn_normal[a][b](i,j) -
dn_normal[a][i-1]*
dn_normal[b][j-1]*2.0 /
n_len) / n_len2);
#if INT_DEBUG > 2
std::cout <<"ElasticCable: ddn_unit:\n";
for (a = 0; a < nen; a++)
for (b = 0; b < nen; b++)
std::cout <<"nodes "<< a+1 <<","<< b+1 << ddn_unit[a][b];
#endif
// Axial strain
double eps = 0.5*(dx*dx - dX*dX);
// Derivative of the axial strain
Vector deps(3*nen);
for (a = aa = 1; a <= nen; a++)
for (i = 1; i <= 3; i++, aa++)
deps(aa) = fe.dNdX(a,1)*dx[i-1];
// Second derivative of the axial strain
Matrix ddeps(3*nen,3*nen);
for (a = 1; a <= nen; a++)
for (b = 1; b <= nen; b++)
for (i = 1; i <= 3; i++)
ddeps(3*(a-1)+i,3*(b-1)+i) = fe.dNdX(a,1)*fe.dNdX(b,1);
// Curvature
double kappa = (ddx*n_unit - ddX*N_unit);
// Derivative of the curvature
Vector dkappa(3*nen);
for (a = aa = 1; a <= nen; a++)
for (i = 1; i <= 3; i++, aa++)
{
dkappa(aa) = fe.d2NdX2(a,1,1)*n_unit[i-1];
for (k = 1; k <= 3; k++)
dkappa(aa) += ddx[k-1]*dn_unit[a-1](k,i);
}
// Second derivative of the curvature
Matrix ddkappa(3*nen,3*nen);
for (a = 0, aa = 1; a < nen; a++)
for (i = 1; i <= 3; i++, aa++)
for (b = 0, bb = 1; b < nen; b++)
for (j = 1; j <= 3; j++, bb++)
{
ddkappa(aa,bb) = (fe.d2NdX2(1+a,1,1)*dn_unit[b](i,j) +
fe.d2NdX2(1+b,1,1)*dn_unit[a](j,i));
for (k = 1; k <= 3; k++)
ddkappa(aa,bb) += ddx[k-1]*ddn_unit[a][b](k,i,j);
}
#if INT_DEBUG > 1
std::cout <<"ElasticCable: eps = "<< eps <<" kappa = "<< kappa
<<"\ndeps:"<< deps <<"dkappa:"<< dkappa
<<"ddeps:"<< ddeps <<"ddkappa:"<< ddkappa;
#endif
// Norm of initial contravariant basis (G^1)
double normG1contr2 = 1.0 / (dX.x*dX.x + dX.y*dX.y + dX.z*dX.z);
double normG1contr4JW = normG1contr2 * normG1contr2 * fe.detJxW;
double EAxJW = EA * normG1contr4JW; // volume-weighted axial stiffness
double EIxJW = EI * normG1contr4JW; // volume-weighted bending stiffness
if (iS)
{
// Integrate the internal forces (note the negative sign here)
elMat.b[iS-1].add(deps,-eps*EAxJW);
elMat.b[iS-1].add(dkappa,-kappa*EIxJW);
}
if (eKm)
{
// Integrate the material stiffness matrix
elMat.A[eKm-1].outer_product(deps,deps*EAxJW,true);
elMat.A[eKm-1].outer_product(dkappa,dkappa*EIxJW,true);
}
if (eKg)
{
// Integrate the geometric stiffness matrix
elMat.A[eKg-1].add(ddeps,eps*EAxJW);
elMat.A[eKg-1].add(ddkappa,kappa*EIxJW);
}
if (lineMass > 0.0)
{
double dMass = lineMass*fe.detJxW;
if (eM)
{
// Integrate the mass matrix
Matrix& M = elMat.A[eM-1];
for (a = 1; a <= nen; a++)
for (b = 1; b <= nen; b++)
for (i = 1; i <= 3; i++)
M(3*(a-1)+i,3*(b-1)+i) += fe.N(a)*fe.N(b)*dMass;
}
if (eS && !gravity.isZero())
{
// Integrate the external (gravitation) forces
Vector& S = elMat.b[eS-1];
for (a = 1; a <= nen; a++)
for (i = 1; i <= 3; i++)
S(3*(a-1)+i) += fe.N(a)*gravity[i-1]*dMass;
}
}
return true;
}
void ObjectiveFunctionTests::compare_numerical_hessian( ObjectiveFunction* of,
bool diagonal_only )
{
const double delta = 0.0001;
MsqPrintError err(std::cout);
PatchData pd;
create_qm_two_tet_patch( pd, err );
ASSERT_NO_ERROR( err );
CPPUNIT_ASSERT( pd.num_free_vertices() != 0 );
// get analytical Hessian from objective function
std::vector<Vector3D> grad;
std::vector<SymMatrix3D> diag;
MsqHessian hess;
hess.initialize( pd, err );
ASSERT_NO_ERROR( err );
double value;
bool valid;
if (diagonal_only)
valid = of->evaluate_with_Hessian_diagonal( ObjectiveFunction::CALCULATE, pd, value, grad, diag, err );
else
valid = of->evaluate_with_Hessian( ObjectiveFunction::CALCULATE, pd, value, grad, hess, err );
ASSERT_NO_ERROR(err); CPPUNIT_ASSERT(valid);
// do numerical approximation of each block and compare to analytical value
for (size_t i = 0; i < pd.num_free_vertices(); ++i) {
const size_t j_end = diagonal_only ? i+1 : pd.num_free_vertices();
for (size_t j = i; j < j_end; ++j) {
// do numerical approximation for block corresponding to
// coorindates for ith and jth vertices.
Matrix3D block;
for (int k = 0; k < 3; ++k) {
for (int m = 0; m < 3; ++m) {
double dk, dm, dkm;
Vector3D ik = pd.vertex_by_index(i);
Vector3D im = pd.vertex_by_index(j);
Vector3D delta_k(0.0); delta_k[k] = delta;
pd.move_vertex( delta_k, i, err ); ASSERT_NO_ERROR(err);
valid = of->evaluate( ObjectiveFunction::CALCULATE, pd, dk, true, err );
ASSERT_NO_ERROR(err); CPPUNIT_ASSERT(valid);
Vector3D delta_m(0.0); delta_m[m] = delta;
pd.move_vertex( delta_m, j, err ); ASSERT_NO_ERROR(err);
valid = of->evaluate( ObjectiveFunction::CALCULATE, pd, dkm, true, err );
ASSERT_NO_ERROR(err); CPPUNIT_ASSERT(valid);
// be careful here that we do the right thing if i==j
pd.set_vertex_coordinates( ik, i, err ); ASSERT_NO_ERROR(err);
pd.set_vertex_coordinates( im, j, err ); ASSERT_NO_ERROR(err);
pd.move_vertex( delta_m, j, err ); ASSERT_NO_ERROR(err);
valid = of->evaluate( ObjectiveFunction::CALCULATE, pd, dm, true, err );
ASSERT_NO_ERROR(err); CPPUNIT_ASSERT(valid);
pd.set_vertex_coordinates( ik, i, err ); ASSERT_NO_ERROR(err);
pd.set_vertex_coordinates( im, j, err ); ASSERT_NO_ERROR(err);
block[k][m] = (dkm - dk - dm + value)/(delta*delta);
}
}
// compare to analytical value
if (diagonal_only) {
CPPUNIT_ASSERT(i == j); // see j_end above
CPPUNIT_ASSERT(i < diag.size());
CHECK_EQUAL_MATRICES( block, Matrix3D(diag[i]) );
}
else {
Matrix3D* m = hess.get_block( i, j );
Matrix3D* mt = hess.get_block( j, i );
if (NULL != m) {
CHECK_EQUAL_MATRICES( block, *m );
}
if (NULL != mt) {
CHECK_EQUAL_MATRICES( transpose(block), *m );
}
if (NULL == mt && NULL == m) {
CHECK_EQUAL_MATRICES( Matrix3D(0.0), block );
}
}
}
}
}
bool TQualityMetric::evaluate_with_Hessian( PatchData& pd,
size_t handle,
double& value,
std::vector<size_t>& indices,
std::vector<Vector3D>& grad,
std::vector<Matrix3D>& Hessian,
MsqError& err )
{
const Sample s = ElemSampleQM::sample( handle );
const size_t e = ElemSampleQM:: elem( handle );
MsqMeshEntity& elem = pd.element_by_index( e );
EntityTopology type = elem.get_element_type();
unsigned edim = TopologyInfo::dimension( type );
size_t num_idx = 0;
const NodeSet bits = pd.non_slave_node_set( e );
bool rval;
if (edim == 3) { // 3x3 or 3x2 targets ?
const MappingFunction3D* mf = pd.get_mapping_function_3D( type );
if (!mf) {
MSQ_SETERR(err)( "No mapping function for element type", MsqError::UNSUPPORTED_ELEMENT );
return false;
}
MsqMatrix<3,3> A, W, dmdT, d2mdT2[6];
mf->jacobian( pd, e, bits, s, mIndices, mDerivs3D, num_idx, A, err );
MSQ_ERRZERO(err);
targetCalc->get_3D_target( pd, e, s, W, err ); MSQ_ERRZERO(err);
const MsqMatrix<3,3> Winv = inverse(W);
const MsqMatrix<3,3> T = A*Winv;
rval = targetMetric->evaluate_with_hess( T, value, dmdT, d2mdT2, err );
MSQ_ERRZERO(err);
gradient<3>( num_idx, mDerivs3D, dmdT*transpose(Winv), grad );
second_deriv_wrt_product_factor( d2mdT2, Winv );
Hessian.resize( num_idx*(num_idx+1)/2 );
if (num_idx)
hessian<3>( num_idx, mDerivs3D, d2mdT2, arrptr(Hessian) );
#ifdef PRINT_INFO
print_info<3>( e, s, A, W, A * inverse(W) );
#endif
}
else if (edim == 2) {
#ifdef NUMERICAL_2D_HESSIAN
// return finite difference approximation for now
return QualityMetric::evaluate_with_Hessian( pd, handle,
value, indices, grad, Hessian,
err );
#else
MsqMatrix<2,2> W, A, dmdT, d2mdT2[3];
MsqMatrix<3,2> M;
rval = evaluate_surface_common( pd, s, e, bits, mIndices, num_idx,
mDerivs2D, W, A, M, err );
if (MSQ_CHKERR(err) || !rval)
return false;
const MsqMatrix<2,2> Winv = inverse(W);
const MsqMatrix<2,2> T = A*Winv;
rval = targetMetric->evaluate_with_hess( T, value, dmdT, d2mdT2, err );
MSQ_ERRZERO(err);
gradient<2>( num_idx, mDerivs2D, M * dmdT * transpose(Winv), grad );
// calculate 2D hessian
second_deriv_wrt_product_factor( d2mdT2, Winv );
const size_t n = num_idx*(num_idx+1)/2;
hess2d.resize(n);
if (n)
hessian<2>( num_idx, mDerivs2D, d2mdT2, arrptr(hess2d) );
// calculate surface hessian as transform of 2D hessian
Hessian.resize(n);
for (size_t i = 0; i < n; ++i)
Hessian[i] = Matrix3D( (M * hess2d[i] * transpose(M)).data() );
#ifdef PRINT_INFO
print_info<2>( e, s, J, Wp, A * inverse(W) );
#endif
#endif
}
else {
assert(0);
return false;
}
// pass back index list
indices.resize( num_idx );
std::copy( mIndices, mIndices+num_idx, indices.begin() );
// apply target weight to value
if (!num_idx)
weight( pd, s, e, num_idx, value, 0, 0, 0, err );
else
weight( pd, s, e, num_idx, value, arrptr(grad), 0, arrptr(Hessian), err );
MSQ_ERRZERO(err);
return rval;
}
