// 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; }