1 //#************************************************************** 2 //# 3 //# filename: linalg3d.h 4 //# 5 //# author: Gerstmayr Johannes 6 //# 7 //# generated: 15.05.97 8 //# description: Classes for linear and nonlinear algebra which is 9 //# thought to be used in finite element or similar applications 10 //# There are 2D and 3D and arbitrary size Vectors (Vector2D, Vector3D, Vector), 11 //# arbitrary size matrices (Matrix), and a nonlinear system (NumNLSys) 12 //# and a nonlinear solver (NumNLSolver) 13 //# remarks: Indizes run from 1 to n except in Vector3D/2D 14 //# 15 //# Copyright (c) 2003-2013 Johannes Gerstmayr, Linz Center of Mechatronics GmbH, Austrian 16 //# Center of Competence in Mechatronics GmbH, Institute of Technical Mechanics at the 17 //# Johannes Kepler Universitaet Linz, Austria. All rights reserved. 18 //# 19 //# This file is part of HotInt. 20 //# HotInt is free software: you can redistribute it and/or modify it under the terms of 21 //# the HOTINT license. See folder 'licenses' for more details. 22 //# 23 //# bug reports are welcome!!! 24 //# WWW: www.hotint.org 25 //# email: bug_reports@hotint.org or support@hotint.org 26 //#************************************************************** 27 28 void Normal3D(const Vector3D& p1,const Vector3D& p2,const Vector3D& p3, Vector3D& n); //normalized normal 29 30 //distance from point p to infinite line (line consists of two points lp1 and lp2) 31 double DistToLine(const Vector3D& lp1, const Vector3D& lp2, const Vector3D& p); 32 double DistToLine(const Vector2D& lp1, const Vector2D& lp2, const Vector2D& p); 33 34 //distance from point p to line defined by linepoint lp and line vector lv; pp is nearest (projected) point on line: 35 //lv must have finite length! 36 double DistToLine(const Vector3D& lp, const Vector3D& lv, const Vector3D& p, Vector3D& pp); 37 38 //intersect two planes (plane normals pn1, pn2; plane points pp1, pp2): result=Line (represented by point lp and line direction lv) 39 //compute the intersection of two planes (if an intersection return 1, if planes are equal return 2, otherwise return 0) 40 int PlaneIntersection(const Vector3D& pn1, const Vector3D& pp1, 41 const Vector3D& pn2, const Vector3D& pp2, 42 Vector3D& lp, Vector3D& lv); 43 //{ 44 //P=[0 0 0]; 45 //N=cross(N1,N2); 46 // 47 //% test if the two planes are parallel 48 //if norm(N) < 10^-7 % Plane 1 and Plane 2 are near parallel 49 // V=A1-A2; 50 // if (dot(N1,V) == 0) 51 // check=1; % Plane 1 and Plane 2 coincide 52 // return 53 // else 54 // check=0; %Plane 1 and Plane 2 are disjoint 55 // return 56 // end 57 //end 58 // 59 // check=2; 60 // 61 //% Plane 1 and Plane 2 intersect in a line 62 //%first determine max abs coordinate of cross product 63 //maxc=find(abs(N)==max(abs(N))); 64 // 65 // 66 //%next, to get a point on the intersection line and 67 //%zero the max coord, and solve for the other two 68 // 69 //d1 = -dot(N1,A1); %the constants in the Plane 1 equations 70 //d2 = -dot(N2, A2); %the constants in the Plane 2 equations 71 // 72 //switch maxc 73 // case 1 % intersect with x=0 74 // P(1)= 0; 75 // P(2) = (d2*N1(3) - d1*N2(3))/ N(1); 76 // P(3) = (d1*N2(2) - d2*N1(2))/ N(1); 77 // case 2 %intersect with y=0 78 // P(1) = (d1*N2(3) - d2*N1(3))/ N(2); 79 // P(2) = 0; 80 // P(3) = (d2*N1(1) - d1*N2(1))/ N(2); 81 // case 3 %intersect with z=0 82 // P(1) = (d2*N1(2) - d1*N2(2))/ N(3); 83 // P(2) = (d1*N2(1) - d2*N1(1))/ N(3); 84 // P(3) = 0; 85 //end 86 // 87 //} 88 89 //distance from line, including boundary: 90 double MinDistLP(const Vector3D& lp1, const Vector3D& lp2, const Vector3D& p); 91 92 //distance from line, including boundary, get projected point: 93 double MinDistLP(const Vector3D& lp1, const Vector3D& lp2, const Vector3D& p, Vector3D& pp); 94 95 double Deflection(const Vector3D& lp1, const Vector3D& lp2, const Vector3D& p, const Vector3D& pn); 96 97 //get deflection in 2D, distance to line segment, right side is positive 98 double RighthandMinDist2D(const Vector2D& lp1, const Vector2D& lp2, const Vector2D& p, Vector2D& pp); 99 100 //Distance of 2 points: 101 double Dist(const Vector3D& p1, const Vector3D& p2); 102 double Dist(const Vector2D& p1, const Vector2D& p2); 103 104 //squared Distance: 105 double Dist2(const Vector3D& p1, const Vector3D& p2); 106 double Dist2(const Vector2D& p1, const Vector2D& p2); 107 108 double NormalizedVectorAngle(const Vector3D& v1, const Vector3D& v2); 109 double VectorAngle(Vector3D v1, Vector3D v2); 110 111 double NormalizedVectorAngle(const Vector2D& v1, const Vector2D& v2); 112 double VectorAngle(Vector2D v1, Vector2D v2); 113 double PolarAngle(double real, double imaginary); 114 double PolarAngle(Vector2D& v); 115 116 // returns parameters k and d of linear equation y = k*x+d, defined by 2 points pi=(xi,yi) 117 Vector2D GetLinearEquationParameters(double x1,double y1,double x2,double y2); //$ DR 2012-04-13 118 119 //Minimal Distance of p to plane (p0,n0), normalized normal, projected point in plane pp: 120 double DistToPlane(const Vector3D& p0, const Vector3D& n0, const Vector3D& p, Vector3D& pp); 121 122 //Minimal Distance of p to plane(HNF) 123 double DistToPlane(const Vector3D& p, const Vector3D& nplane, double cplane); 124 double DistToPlane(const Vector2D& p, const Vector2D& nplane, double cplane); 125 double DistToPlane(const Vector3D& p, const Vector3D& nplane, const Vector3D& n0); 126 double DistToPlane(const Vector2D& p, const Vector2D& nplane, const Vector2D& n0); 127 128 //Mirror Point/Vactor at plane(HNF) 129 Vector3D MirrorAtPlane3D(const Vector3D& p, const Vector3D& nplane, double cplane); 130 131 //Minimal Distance to Triangle: 132 double DistToTrig(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, 133 const Vector3D& p); 134 //Minimal Distance to Quad: 135 double DistToQuad(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, const Vector3D& p4, 136 const Vector3D& p); 137 138 //compute local coordinates lam1, lam2 of a vector v in a triangle: 139 void LocalCoordinates (const Vector3D & e1, const Vector3D & e2, 140 const Vector3D & v, double & lam1, double & lam2); 141 142 //minimum distance between point p and triangle 143 double MinDistTP(const Vector3D & tp1, const Vector3D & tp2, 144 const Vector3D & tp3, const Vector3D & p); 145 146 //minimum distance between point p and triangle 147 double MinDistTP(const Vector3D& tp1, const Vector3D& tp2, 148 const Vector3D& tp3, const Vector3D& p, Vector3D& pp); 149 150 void ProjectInPlane(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, Vector3D& pp); 151 void ProjectInPlane(const Vector3D& p1, const Vector3D& n, Vector3D& pp); 152 153 //return intersection point; rv=1 if success, rv=0 if no success, means line is normal to plane normal; pline is projected in direction of vline in plane 154 int IntersectLineWithPlane(const Vector3D& pplane, const Vector3D& nplane, const Vector3D& vline, Vector3D& pline); 155 156 //int PointInside(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, const Vector3D& pp); 157 //double MinDistTP2(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, const Vector3D& p3d); 158 159 double NormSym(const Matrix3D& m); 160 Matrix3D Deviator(const Matrix3D& m); 161 double DoubleProd3D(const Matrix3D& m, const Matrix3D& n); 162 163 double Area(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3); 164 double Area(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, const Vector3D& p4); 165 166 167 ////// compute intersection of two lines in vector paramater representation 168 ////// returns 1 on success , -1 on skew lines(pti is not changed) 169 ////// returns 0 when identical(returns pti == pt1), -1 when parallel(pti is not changed) 170 ////int IntersectionPoint( Vector3D pt1, Vector3D dir1, Vector3D pt2, Vector3D dir2, Vector3D& pti ); 171 172 // compute intersection of two lines in vector paramater representation 173 // returns 1 on success 174 // returns 2 for identical lines 175 // retirns 0 for parallel or skew lines(pti is not changed) 176 int IntersectLines3D( Vector3D pt1, Vector3D dir1, Vector3D pt2, Vector3D dir2, Vector3D& pti, Vector2D& lambda_mu ); 177 178 // compute intersection of two lines in vector paramater representation 179 // returns 1 on success 180 // returns 2 for identical lines 181 // retirns 0 for parallel or skew lines(pti is not changed) 182 int IntersectLines2D( Vector2D pt1, Vector2D dir1, Vector2D pt2, Vector2D dir2, Vector2D& pti, Vector2D& lambda_mu ); 183 184 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 185 //Rotation parameters: 186 187 //compute rotation matrix from Quaternions (euler parameters); b0 ..b3 are the euler parameters 188 Matrix3D ComputeRotMatrixFromQuaternions(double b0, double b1, double b2, double b3); 189 190 191 Matrix3D ComputeRotMatrixEulerParam(const double& beta0, const double& beta1, 192 const double& beta2, const double& beta3); 193 194 Matrix3D ComputeRotMatrixPEulerParam(const double& beta0, const double& beta1, 195 const double& beta2, const double& beta3, const double& betap0, const double& betap1, 196 const double& betap2, const double& betap3); 197 198 Matrix3D ComputeRotMatrixEuler(const double phi1, const double phi2, const double phi3); 199 200 Matrix3D ComputeRotMatrixWithKardanAngles(const double phi1, const double phi2, const double phi3); //this is the HOTINT Kardan angle definition from local to global coordinates 201 202 void RotMatToKardanAngles(const Matrix3D& A, Vector3D& phi); //this function gives all angles between -pi to +pi 203 204 void QuaternionsToKardanAngles(double beta0, double beta1, double beta2, double beta3, Vector3D& phi);//RL 205 206 Matrix3D ComputeRotMatrixEulerP(const double& phi1, const double& phi2, const double& phi3, 207 const double& phi1p, const double& phi2p, const double& phi3p); 208 209 void QuaternionsToEulerAngles(double beta0, double beta1, double beta2, double beta3, Vector3D& phi); 210 211 void RotMatToEulerAngles(const Matrix3D& A, Vector3D& phi); 212 213 void QuaternionsPToAngularVelocities(double beta0, double beta1, double beta2, double beta3, 214 double beta0p, double beta1p, double beta2p, double beta3p, Vector3D& phip); 215 216 void RotMatToQuaternions(const Matrix3D& A, double& beta0, double& beta1, double& beta2, double& beta3); 217 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 218 219 Matrix DH_ij(double a, double alpha, double d, double theta); // compute denavit hartenberg transformaion matrix from dh-parameters 220 void GetAij(const Matrix* T4_ij,Matrix3D& Aij); // compute rotation matrix from 4x4 transformation matrix 221 void Get_ri_jo(const Matrix* T4_ij,Vector3D& ri_jo); // compute displacement vector from 4x4 transformation matrix 222 void GetT4x4_ij(const Matrix3D& Aij,const Vector3D& ri_jo, Matrix* T4_ij); // compute 4x4 transformation matrix from rotation matrix and displacement vector 223 224 225 //+++++++++++++++++++++++++++++++++++++++++ 226 227 //compute rotation matrices with respect to axis 1, 2 and 3 228 Matrix3D RotMatrix1(double p); 229 Matrix3D RotMatrix2(double p); 230 Matrix3D RotMatrix3(double p); 231 232 double GetRotation(int rotaxis, const Matrix3D& rotmat); 233 234 class Vector3D BaseVector3D(int i); 235 class Vector2D BaseVector2D(int i); 236 //----------------------------------------------------------------- 237 //-------------- CLASS VECTOR 2D ----------------------------- 238 //----------------------------------------------------------------- 239 //Generation of a vector with user-defined length 240 //Operations: +,-,* (also matrices), << 241 //initialized with 0 !!! 242 class Vector2D 243 { 244 public: 245 246 //Constructors, Destructor Vector2D()247 Vector2D() { vec[0]=0; vec[1]=0;}; Vector2D(double x)248 Vector2D(double x) { vec[0]=x; vec[1]=x; }; Vector2D(double x,double y)249 Vector2D(double x, double y) 250 { 251 vec[0]=x; vec[1]=y; 252 } Vector2D(const Vector2D & veci)253 Vector2D(const Vector2D& veci) 254 { 255 vec[0] = veci[0]; vec[1] = veci[1]; 256 } 257 // Vector2D( Vector3D& veci); 258 Vector3D MakeV3D() const; 259 //virtual ~Vector2D() { }; 260 GetVecPtr()261 const double* GetVecPtr() const {return &vec[0];} GetVecPtr()262 double* GetVecPtr() {return &vec[0];} 263 264 //Assignment-operator 265 Vector2D& operator= (const Vector2D& veci) 266 { 267 if (&veci == this) return *this; 268 269 vec[0] = veci[0]; 270 vec[1] = veci[1]; 271 return *this; 272 } 273 274 Vector2D& operator= (const double& val) 275 { 276 vec[0] = val; 277 vec[1] = val; 278 return *this; 279 } 280 //Logical operator 281 int operator== (const Vector2D& veci) 282 { 283 return (vec[0] == veci[0] && 284 vec[1] == veci[1]); 285 } 286 X()287 double& X() {return vec[0];} Y()288 double& Y() {return vec[1];} 289 X()290 const double& X() const {return vec[0];} Y()291 const double& Y() const {return vec[1];} 292 293 //Referencing access-operator 294 double& operator[](int elem) 295 { 296 #ifndef __QUICKMATH 297 release_assert((elem>=0) && (elem<2)); 298 #endif 299 return vec[elem]; 300 }; 301 //Referencing access-operator 302 const double& operator[](int elem) const 303 { 304 #ifndef __QUICKMATH 305 release_assert((elem>=0) && (elem<2)); 306 #endif 307 return vec[elem]; 308 }; 309 //Referencing access-operator operator()310 double& operator()(int elem) 311 { 312 #ifndef __QUICKMATH 313 release_assert((elem>0) && (elem<=2)); 314 #endif 315 return vec[elem-1]; 316 }; 317 //Referencing access-operator operator()318 const double& operator()(int elem) const 319 { 320 #ifndef __QUICKMATH 321 release_assert((elem>0) && (elem<=2)); 322 #endif 323 return vec[elem-1]; 324 }; Set(const double & x,const double & y)325 void Set(const double& x, const double& y) 326 { 327 vec[0]=x; vec[1]=y; 328 } Get(double & x,double & y)329 void Get(double& x, double& y) //$JG2012-01: this is for easier access to vector3d data 330 { 331 x=vec[0]; y=vec[1]; 332 } 333 334 //Returns the length of a vector Length()335 int Length() const {return 2; }; GetLen()336 int GetLen() const {return 2; }; 337 Cross(const Vector2D & v)338 double Cross(const Vector2D& v) const 339 { 340 return vec[0]*v.vec[1]-vec[1]*v.vec[0]; 341 } 342 //Returns the quadratic norm of a vector Norm()343 double Norm() const {return sqrt(vec[0]*vec[0]+vec[1]*vec[1]);} 344 //Returns the quadratic norm of a vector squared Norm2()345 double Norm2() const {return vec[0]*vec[0]+vec[1]*vec[1];} 346 347 //norms the vector Normalize()348 void Normalize() 349 { 350 double l=Norm(); 351 if (l>1e-16) {vec[0]/=l;vec[1]/=l;} 352 } 353 354 //Returns the maximum-norm of a vector MaxNorm()355 double MaxNorm() const {return Maximum(vec[0],vec[1]);} 356 357 //Returns the minimum-norm of a vector MinNorm()358 double MinNorm() const {return Minimum(vec[0],vec[1]);}; 359 Scale(double x,double y)360 void Scale(double x, double y) 361 { 362 vec[0]/=x; vec[1]/=y; 363 } 364 365 //Arithmetic operations with one parameter 366 Vector2D operator+= (const Vector2D& v) 367 { 368 vec[0]+=v.vec[0]; 369 vec[1]+=v.vec[1]; 370 return *this; 371 } 372 Vector2D operator-= (const Vector2D& v) 373 { 374 vec[0]-=v.vec[0]; 375 vec[1]-=v.vec[1]; 376 return *this; 377 } 378 379 Vector2D operator*= (const double& v) 380 { 381 vec[0]*=v; 382 vec[1]*=v; 383 return *this; 384 } 385 386 Vector2D operator/= (const double& v) 387 { 388 vec[0]/=v; 389 vec[1]/=v; 390 return *this; 391 } 392 393 //Arithmetic operations with two parameters 394 friend Vector2D operator+ (const Vector2D& v1, const Vector2D& v2) 395 { 396 return Vector2D(v1.vec[0]+v2.vec[0], 397 v1.vec[1]+v2.vec[1]); 398 } 399 friend Vector2D operator- (const Vector2D& v1, const Vector2D& v2) 400 { 401 return Vector2D(v1.vec[0]-v2.vec[0], 402 v1.vec[1]-v2.vec[1]); 403 } 404 friend Vector2D operator* (const Vector2D& v, const double& val) 405 { 406 return Vector2D(v.vec[0]*val, 407 v.vec[1]*val); 408 } 409 410 friend Vector2D operator* (const double& val, const Vector2D& v) 411 { 412 return Vector2D(v.vec[0]*val, 413 v.vec[1]*val); 414 } 415 friend double operator* (const Vector2D& v1, const Vector2D& v2) 416 { 417 return v1.vec[0]*v2.vec[0]+v1.vec[1]*v2.vec[1]; 418 } 419 420 friend Vector2D operator* (const Matrix& m, const Vector2D& v); 421 friend Vector2D operator* (const Matrix3D& m, const Vector2D& v); 422 friend Vector2D operator* (const Matrix2D& m, const Vector2D& v); 423 friend Vector2D operator* (const Vector2D& v, const Matrix2D& m); //res=v^T*m =m^T*v 424 friend Vector2D operator* (const Matrix2D& m, const Vector& v); 425 friend void Mult(const Matrix2D& m, const Vector2D& v, Vector& res); 426 friend void Mult(const Matrix& m, const Vector2D& v, Vector& res); //computes res=m*v 427 428 //Output parameter 429 friend ostream& operator<<(ostream& os, const Vector2D& v) 430 { 431 os << "[" << v.X() << ", " << v.Y() << "]"; 432 return os; 433 } 434 435 private: 436 double vec[2]; 437 438 }; 439 440 441 //----------------------------------------------------------------- 442 //-------------- CLASS VECTOR 3D ----------------------------- 443 //----------------------------------------------------------------- 444 //Generation of a vector with user-defined length 445 //Operations: +,-,* (also matrices), << 446 //initialized with 0 !!! 447 class Vector3D 448 { 449 public: 450 451 //Constructors, Destructor Vector3D()452 Vector3D() { vec[0]=0; vec[1]=0; vec[2]=0;}; Vector3D(double x)453 Vector3D(double x) { vec[0]=x; vec[1]=x; vec[2]=x;}; Vector3D(double x,double y,double z)454 Vector3D(double x, double y, double z) 455 { 456 vec[0]=x; vec[1]=y; vec[2]=z; 457 } Vector3D(const Vector3D & avec)458 Vector3D(const Vector3D& avec) 459 { 460 vec[0] = avec.X(); vec[1] = avec.Y(); vec[2] = avec.Z(); 461 } 462 463 // Vector3D(const Vector2D& avec); 464 Vector2D MakeV2D() const; 465 466 // virtual ~Vector3D() { }; 467 468 //Assignment-operator 469 Vector3D& operator= (const Vector3D& veci) 470 { 471 if (&veci == this) return *this; 472 473 vec[0] = veci[0]; 474 vec[1] = veci[1]; 475 vec[2] = veci[2]; 476 return *this; 477 } 478 479 Vector3D& operator= (const double& val) 480 { 481 vec[0] = val; 482 vec[1] = val; 483 vec[2] = val; 484 return *this; 485 } 486 //Logical operator 487 int operator== (const Vector3D& veci) 488 { 489 return (vec[0] == veci[0] && 490 vec[1] == veci[1] && 491 vec[2] == veci[2]); 492 } 493 494 int operator== (const Vector3D& veci) const 495 { 496 return (vec[0] == veci[0] && 497 vec[1] == veci[1] && 498 vec[2] == veci[2]); 499 } 500 X()501 double& X() {return vec[0];} Y()502 double& Y() {return vec[1];} Z()503 double& Z() {return vec[2];} 504 X()505 const double& X() const {return vec[0];} Y()506 const double& Y() const {return vec[1];} Z()507 const double& Z() const {return vec[2];} 508 509 //Referencing access-operator 510 double& operator[](int elem) 511 { 512 #ifndef __QUICKMATH 513 release_assert((elem>=0) && (elem<3)); 514 #endif 515 return vec[elem]; 516 }; 517 //Referencing access-operator 518 const double& operator[](int elem) const 519 { 520 #ifndef __QUICKMATH 521 release_assert((elem>=0) && (elem<3)); 522 #endif 523 return vec[elem]; 524 }; 525 //Referencing access-operator operator()526 double& operator()(int elem) 527 { 528 #ifndef __QUICKMATH 529 release_assert((elem>0) && (elem<=3)); 530 #endif 531 return vec[elem-1]; 532 }; 533 //Referencing access-operator operator()534 const double& operator()(int elem) const 535 { 536 #ifndef __QUICKMATH 537 release_assert((elem>0) && (elem<=3)); 538 #endif 539 return vec[elem-1]; 540 }; 541 GetVecPtr()542 const double* GetVecPtr() const {return &vec[0];} GetVecPtr()543 double* GetVecPtr() {return &vec[0];} 544 //Returns the length of a vector Length()545 int Length() const {return 3; }; GetLen()546 int GetLen() const {return 3; }; Set(const double & x,const double & y,const double & z)547 void Set(const double& x, const double& y, const double& z) 548 { 549 vec[0]=x; vec[1]=y; vec[2]=z; 550 } Get(double & x,double & y,double & z)551 void Get(double& x, double& y, double& z) //$JG2012-01: this is for easier access to vector3d data 552 { 553 x=vec[0]; y=vec[1]; z=vec[2]; 554 } 555 556 //Returns the quadratic norm of a vector Norm()557 double Norm() const {return sqrt(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]);} 558 //Returns the quadratic norm of a vector squared Norm2()559 double Norm2() const {return vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2];} 560 561 //norms the vector Normalize()562 void Normalize() 563 { 564 double l=Norm(); 565 if (l!=0.) {vec[0]/=l;vec[1]/=l;vec[2]/=l;} 566 } Scale(double x,double y,double z)567 void Scale(double x, double y, double z) 568 { 569 vec[0]/=x; vec[1]/=y; vec[2]/=z; 570 } 571 //normalizes *this and gets some orthogonal vectors n1 and n2 SetNormalBasis(Vector3D & n1,Vector3D & n2)572 void SetNormalBasis(Vector3D& n1, Vector3D& n2) 573 { 574 Normalize(); 575 Vector3D nx; 576 if (fabs(vec[0]) > 0.5 && fabs(vec[1]) < 0.1 && fabs(vec[2]) < 0.1) nx.Set(0.,1.,0.); 577 else nx.Set(1.,0.,0.); 578 579 double h = nx*(*this); 580 n1 = nx-h*(*this); 581 n1.Normalize(); 582 n2=this->Cross(n1); 583 } 584 585 //Project n into normal plane of *this GramSchmidt(Vector3D & n)586 void GramSchmidt(Vector3D& n) 587 { 588 double h = n*(*this)/((*this)*(*this)); 589 n -= h*(*this); 590 n.Normalize(); 591 } 592 593 //Project n into normal plane of *this 594 //PG: added const-version of this routine GramSchmidt(Vector3D & n)595 void GramSchmidt(Vector3D& n) const 596 { 597 double h = n*(*this)/((*this)*(*this)); 598 n -= h*(*this); 599 n.Normalize(); 600 } 601 602 //Returns the maximum-norm of a vector MaxNorm()603 double MaxNorm() const {return Maximum(vec[0],vec[1],vec[2]);} 604 605 //Returns the minimum-norm of a vector MinNorm()606 double MinNorm() const {return Minimum(vec[0],vec[1],vec[2]);}; 607 608 //Returns the normalized planar perpendicular vector of a vector Cross(const Vector3D & v)609 Vector3D Cross(const Vector3D& v) const 610 { 611 return Vector3D(vec[1]*v.vec[2]-vec[2]*v.vec[1], 612 vec[2]*v.vec[0]-vec[0]*v.vec[2], 613 vec[0]*v.vec[1]-vec[1]*v.vec[0]); 614 } 615 616 mystr& MakeString(mystr intro = mystr("")); 617 618 //Arithmetic operations with one parameter 619 Vector3D operator+= (const Vector3D& v) 620 { 621 vec[0]+=v.vec[0]; 622 vec[1]+=v.vec[1]; 623 vec[2]+=v.vec[2]; 624 return *this; 625 } 626 Vector3D operator-= (const Vector3D& v) 627 { 628 vec[0]-=v.vec[0]; 629 vec[1]-=v.vec[1]; 630 vec[2]-=v.vec[2]; 631 return *this; 632 } 633 634 Vector3D operator*= (const double& v) 635 { 636 vec[0]*=v; 637 vec[1]*=v; 638 vec[2]*=v; 639 return *this; 640 } 641 642 Vector3D operator/= (const double& v) 643 { 644 vec[0]/=v; 645 vec[1]/=v; 646 vec[2]/=v; 647 return *this; 648 } 649 650 //Arithmetic operations with two parameters 651 friend Vector3D operator+ (const Vector3D& v1, const Vector3D& v2) 652 { 653 return Vector3D(v1.vec[0]+v2.vec[0], 654 v1.vec[1]+v2.vec[1], 655 v1.vec[2]+v2.vec[2]); 656 } 657 friend Vector3D operator- (const Vector3D& v1, const Vector3D& v2) 658 { 659 return Vector3D(v1.vec[0]-v2.vec[0], 660 v1.vec[1]-v2.vec[1], 661 v1.vec[2]-v2.vec[2]); 662 } 663 friend Vector3D operator* (const Vector3D& v, const double& val) 664 { 665 return Vector3D(v.vec[0]*val, 666 v.vec[1]*val, 667 v.vec[2]*val); 668 } 669 670 friend Vector3D operator* (const double& val, const Vector3D& v) 671 { 672 return Vector3D(v.vec[0]*val, 673 v.vec[1]*val, 674 v.vec[2]*val); 675 } 676 friend double operator* (const Vector3D& v1, const Vector3D& v2) 677 { 678 return v1.vec[0]*v2.vec[0]+v1.vec[1]*v2.vec[1]+v1.vec[2]*v2.vec[2]; 679 } 680 681 friend Vector3D operator* (const Matrix& m, const Vector3D& v); 682 friend Vector3D operator* (const Matrix3D& m, const Vector3D& v); 683 friend Vector3D operator* (const Vector3D& v, const Matrix3D& m); //res=v^T*m =m^T*v 684 friend Vector3D operator* (const Matrix3D& m, const Vector& v); 685 friend Vector3D operator* (const Matrix3D& m, const Vector& v); 686 687 friend void Mult(const Matrix3D& m, const Vector3D& v, Vector& res); 688 friend void Mult(const Matrix& m, const Vector3D& v, Vector& res); //computes res=m*v 689 friend void Mult(const MatrixXD& m, const Vector& v, Vector3D& res); 690 friend void Mult(const Matrix3D& m, const Vector3D& v, Vector3D& res); 691 friend void Mult(const Matrix& m, const Vector& v, Vector3D& res); //computes res=m*v 692 friend void MultTp(const Matrix& m, const Vector& v, Vector3D& res); //computes res=m*v 693 694 friend void StrainVectorToMatrix2D(Matrix2D& m, const Vector3D& v); 695 696 //Output parameter 697 friend ostream& operator<<(ostream& os, const Vector3D& v) 698 { 699 os << "[" << v.X() << ", " << v.Y() << ", " << v.Z() << "]"; 700 return os; 701 } 702 703 private: 704 double vec[3]; 705 706 }; 707 708 709 //----------------------------------------------------------------- 710 //CLASS MatrixXD CLASS MatrixXD CLASS MatrixXD CLASS MatrixXD 711 //----------------------------------------------------------------- 712 //Generation of a Matrix with user-defined size 713 //Operations: +,-,*,<< 714 class MatrixXD 715 { 716 protected: 717 int rows; 718 int cols; 719 double mat[16]; 720 public: 721 722 //Constructors,destructor MatrixXD()723 MatrixXD() 724 { 725 rows = 0; cols = 0; 726 } 727 728 //fill Diagonal matrix with x MatrixXD(double x)729 MatrixXD(double x) 730 { 731 rows = 0; cols = 0; 732 } 733 734 // copy constructor MatrixXD(const MatrixXD & mati)735 MatrixXD(const MatrixXD& mati) 736 { 737 cols = mati.cols; rows = mati.rows; 738 739 for (int i=0; i < cols*rows; i++) 740 { 741 mat[i] = mati.mat[i]; 742 } 743 } 744 ~MatrixXD()745 virtual ~MatrixXD() {}; 746 Getcols()747 virtual int Getcols() const {return cols;} Getrows()748 virtual int Getrows() const {return rows;} GetMatPtr()749 virtual double* GetMatPtr() {return mat;} 750 751 //Assignment-operator 752 virtual MatrixXD& operator= (const MatrixXD& mati) 753 { 754 rows=mati.rows; 755 cols=mati.cols; 756 for (int i=0; i < rows*cols; i++) 757 { 758 mat[i] = mati.mat[i]; 759 } 760 return *this; 761 } 762 SetAll(double x)763 virtual void SetAll(double x) 764 { 765 for (int i=0; i < rows*cols; i++) 766 { 767 mat[i] = x; 768 } 769 } 770 SetSize(int r,int c)771 virtual void SetSize(int r, int c) {rows = r; cols = c;} 772 773 //fill Diagonal matrix with x -- pure virtual: overwrite in derived class 774 virtual void SetDiag(double x) = 0; 775 776 //returnsum of absolute values AbsNorm()777 virtual double AbsNorm() const 778 { 779 double val = 0.; 780 781 for (int i=0; i < rows*cols; i++) 782 { 783 val += fabs(mat[i]); 784 } 785 786 return val; 787 } 788 789 //Returns Determinate of matrix Det()790 virtual double Det() const 791 { 792 if (cols == 3) 793 { 794 return mat[0]*mat[4]*mat[8]-mat[0]*mat[5]*mat[7]-mat[3]*mat[1]*mat[8] 795 +mat[3]*mat[2]*mat[7]+mat[6]*mat[1]*mat[5]-mat[6]*mat[2]*mat[4]; 796 } 797 else 798 { 799 return mat[0]*mat[3]-mat[1]*mat[2]; 800 } 801 } 802 Trace()803 virtual double Trace() const 804 { 805 double sum=0; 806 for (int i=0; i < min(rows,cols); i++) 807 { 808 sum += Get0(i,i); 809 } 810 return sum; 811 } 812 813 // synonim of DoubleContract InnerProduct(const MatrixXD & rhs)814 virtual double InnerProduct(const MatrixXD& rhs) const 815 { 816 #ifndef __QUICKMATH 817 release_assert(rows == rhs.rows); 818 release_assert(cols == rhs.rows); 819 #endif 820 double val = 0.; 821 for(int i = 0; i < rows*cols; i++) 822 { 823 val += mat[i]*rhs.mat[i]; 824 } 825 return val; 826 } 827 828 //transpose me TpYs()829 virtual void TpYs() 830 { 831 #ifndef __QUICKMATH 832 release_assert(rows == cols); 833 #endif 834 for (int i=1; i<rows; i++) 835 { 836 for (int j=0; j<i; j++) 837 { 838 swap(mat[j*rows+i],mat[i*cols+j]); 839 } 840 } 841 } 842 MakeSym()843 virtual void MakeSym() //make matrix symmetric: (A+A^T)/2 844 { 845 #ifndef __QUICKMATH 846 release_assert(rows==cols); 847 #endif 848 for (int i=0; i < cols; i++) 849 { 850 for (int j=0; j < i; j++) 851 { 852 Get0(i,j) += Get0(j,i); 853 Get0(i,j) *= 0.5; 854 Get0(j,i) = Get0(i,j); 855 } 856 } 857 } 858 859 // synonim of InnerProduct DoubleContract(const MatrixXD & m)860 virtual double DoubleContract(const MatrixXD& m) const 861 { 862 return InnerProduct(m); 863 } 864 865 //Referencing access-operator on element using row- and column-values, 0-based!!! Get0(int row,int col)866 virtual double& Get0(int row, int col) 867 { 868 return mat[row*cols+col]; 869 }; 870 871 //Referencing constant access-operator on element using row- and column-values, 0-based!!! Get0(int row,int col)872 virtual const double& Get0(int row, int col) const 873 { 874 return mat[row*cols+col]; 875 }; 876 877 878 //Referencing access-operator on element using row- and column-values, 1-based!!! operator()879 virtual double& operator()(int row, int col) 880 { 881 return mat[(row-1)*cols+col-1]; 882 }; 883 884 //Referencing constant access-operator on element using row- and column-values, 1-based!!! operator()885 virtual const double& operator()(int row, int col) const 886 { 887 return mat[(row-1)*cols+col-1]; 888 }; 889 890 Transpose()891 virtual void Transpose() 892 { 893 #ifndef __QUICKMATH 894 release_assert(rows==cols); 895 #endif 896 double help; 897 int idx1; 898 int idx2; 899 900 for(int i=0; i < rows; i++) 901 { 902 for(int j=0; j < i; j++) 903 { 904 idx1 = i*cols + j; 905 idx2 = j*cols + i; 906 907 help = mat[idx1]; 908 mat[idx1] = mat[idx2]; 909 mat[idx2] = help; 910 } 911 } 912 913 } 914 915 mystr& MakeString(mystr intro = mystr("")); 916 InnerProduct(const MatrixXD & m1,const MatrixXD & m2)917 friend double InnerProduct(const MatrixXD& m1, const MatrixXD& m2) { return m1.InnerProduct(m2); } 918 friend void Mult(const MatrixXD& m, const Vector& v, Vector& res); 919 friend void Mult(const MatrixXD& m1, const Matrix& m2, Matrix& res); 920 friend void Mult(const Matrix& m1, const MatrixXD& m2, Matrix& res); 921 922 //writes out MatrixXD with constant width and factor normalized 923 friend ostream& operator<<(ostream& os, const MatrixXD& m); 924 }; 925 926 927 928 //----------------------------------------------------------------- 929 //CLASS Matrix2D CLASS Matrix2D CLASS Matrix2D CLASS Matrix2D 930 //----------------------------------------------------------------- 931 932 class Matrix2D : public MatrixXD 933 { 934 public: 935 Matrix2D()936 Matrix2D() 937 { 938 rows = 2; cols = 2; 939 940 mat[0] = 0.; mat[1] = 0.; 941 mat[2] = 0.; mat[3] = 0.; 942 } 943 Matrix2D(double x1)944 Matrix2D(double x1) 945 { 946 rows = 2; cols = 2; 947 948 mat[0] = x1; mat[1] = 0.; 949 mat[2] = 0.; mat[3] = x1; 950 } 951 Matrix2D(double x1,double x2)952 Matrix2D(double x1, double x2) 953 { 954 rows=2; cols=2; 955 956 mat[0] = x1; mat[1] = 0.; 957 mat[2] = 0.; mat[3] = x2; 958 } 959 Matrix2D(double x11,double x12,double x21,double x22)960 Matrix2D( 961 double x11, double x12, 962 double x21, double x22) 963 { 964 rows=2; cols=2; 965 966 mat[0] = x11; mat[1] = x12; 967 mat[2] = x21; mat[3] = x22; 968 } 969 SetDiag(double x)970 void SetDiag(double x) 971 { 972 rows = 2; cols = 2; 973 974 mat[0]=x; mat[1]=0; 975 mat[2]=0; mat[3]=x; 976 } 977 978 //set Matrix as Skew matrix (to rotate a Vector2D around an angle phi) SetSkew(double phi)979 void SetSkew(double phi) 980 { 981 rows = 2; cols = 2; 982 983 double cos_phi = cos(phi); 984 double sin_phi = sin(phi); 985 986 mat[0] = cos_phi; mat[1] = -sin_phi; 987 mat[2] = sin_phi; mat[3] = cos_phi; 988 } 989 Set(const Vector2D & r1,const Vector2D & r2)990 void Set(const Vector2D& r1, const Vector2D& r2) 991 { 992 mat[0] = r1.X(); mat[1] = r2.X(); 993 mat[2] = r1.Y(); mat[3] = r2.Y(); 994 } 995 996 //only for 2x2, fast!!! GetATA2(MatrixXD & m)997 void GetATA2(MatrixXD& m) 998 { 999 for (int i=0;i<2;i++) 1000 { 1001 for (int j=i;j<2;j++) 1002 { 1003 m.Get0(i,j) = 0.5*(mat[i]*mat[j] + mat[2+i]*mat[2+j]); 1004 } 1005 } 1006 // 0 1 1007 // 2 3 1008 m.Get0(1,0) = m.Get0(0,1); 1009 } 1010 GetTp()1011 Matrix2D GetTp() const 1012 { 1013 Matrix2D mt; 1014 mt.SetSize(cols, rows); 1015 1016 for (int i=0; i<rows; i++) 1017 { 1018 for (int j=0; j<cols; j++) 1019 { 1020 mt.Get0(i,j)=Get0(j,i); 1021 } 1022 } 1023 return mt; 1024 } 1025 1026 int GetInverse(Matrix2D& m2); Invert()1027 int Invert() 1028 { 1029 Matrix2D m2; 1030 int rv = GetInverse(m2); 1031 1032 if (rv) 1033 { 1034 *this = m2; 1035 } 1036 1037 return rv; 1038 } 1039 1040 //Arithmetic operations with 1 parameter 1041 Matrix2D& operator+= (const Matrix2D& m) 1042 { 1043 #ifndef __QUICKMATH 1044 release_assert(rows == m.Getrows()); 1045 release_assert(cols == m.Getcols()); 1046 #endif 1047 for (int i=0; i < rows; i++) 1048 { 1049 for (int j=0; j < cols; j++) 1050 { 1051 Get0(i,j) += m.Get0(i,j); 1052 } 1053 } 1054 return *this; 1055 } 1056 1057 Matrix2D& operator-= (const Matrix2D& m) 1058 { 1059 #ifndef __QUICKMATH 1060 release_assert(rows == m.Getrows()); 1061 release_assert(cols == m.Getcols()); 1062 #endif 1063 for (int i=0; i < rows; i++) 1064 { 1065 for (int j=0; j < cols; j++) 1066 { 1067 Get0(i,j) -= m.Get0(i,j); 1068 } 1069 } 1070 return *this; 1071 } 1072 1073 Matrix2D& operator*= (const double& val) 1074 { 1075 for (int i=0; i < rows; i++) 1076 { 1077 for (int j=0; j < cols; j++) 1078 { 1079 Get0(i,j) *= val; 1080 } 1081 } 1082 return *this; 1083 } 1084 1085 //get a orthogonal matrix from vectors v1 and v2 SetOrthogonalBasis(const Vector2D & v1,const Vector2D & v2)1086 void SetOrthogonalBasis(const Vector2D& v1, const Vector2D& v2) 1087 { 1088 Vector2D r1 = v1; 1089 Vector2D r2 = v2; 1090 r1.Normalize(); 1091 1092 double h = r2*r1; 1093 r2 -= h*r1; 1094 r2.Normalize(); 1095 1096 Set(r1,r2); 1097 } 1098 1099 //Arithmetic operations with 2 parameters 1100 friend Matrix2D operator+ (const Matrix2D& m1, const Matrix2D& m2); 1101 friend Matrix2D operator- (const Matrix2D& m1, const Matrix2D& m2); 1102 friend Matrix2D operator* (const Matrix2D& m1, const double& val); 1103 friend Matrix2D operator* (const double& val, const Matrix2D& m1); 1104 friend Matrix2D operator* (const Matrix2D& m1, const Matrix2D& m2); 1105 friend Vector2D operator* (const Matrix2D& m, const Vector2D& v); 1106 friend Vector2D operator* (const Vector2D& v, const Matrix2D& m); //res=v^T*m =m^T*v 1107 friend Vector2D operator* (const Matrix2D& m, const Vector& v); 1108 1109 friend void Mult(const Matrix2D& m, const Vector2D& v, Vector& res); 1110 1111 //transform strain and stress vectors to matrices and vice versa (by PG) 1112 friend void StrainVectorToMatrix2D(Matrix2D& m, const Vector& v); 1113 friend void StrainVectorToMatrix2D(Matrix2D& m, const Vector3D& v); 1114 friend void StressVectorToMatrix2D(Matrix2D& m, const Vector& v); 1115 friend void Matrix2DToStrainVector(Vector& v, const Matrix2D& m); 1116 friend void Matrix2DToStressVector(Vector& v, const Matrix2D& m); 1117 }; 1118 1119 1120 1121 1122 //----------------------------------------------------------------- 1123 //CLASS Matrix3D CLASS Matrix3D CLASS Matrix3D CLASS Matrix3D 1124 //----------------------------------------------------------------- 1125 1126 class Matrix3D : public MatrixXD 1127 { 1128 public: 1129 Matrix3D()1130 Matrix3D() 1131 { 1132 rows = 3; cols = 3; 1133 1134 mat[0] = 0.; mat[1] = 0.; mat[2] = 0.; 1135 mat[3] = 0.; mat[4] = 0.; mat[5] = 0.; 1136 mat[6] = 0.; mat[7] = 0.; mat[8] = 0.; 1137 } 1138 Matrix3D(double x1)1139 Matrix3D(double x1) 1140 { 1141 rows = 3; cols = 3; 1142 1143 mat[0] = x1; mat[1] = 0.; mat[2] = 0.; 1144 mat[3] = 0.; mat[4] = x1; mat[5] = 0.; 1145 mat[6] = 0.; mat[7] = 0.; mat[8] = x1; 1146 } 1147 Matrix3D(double x1,double x2,double x3)1148 Matrix3D(double x1, double x2, double x3) 1149 { 1150 rows = 3; cols = 3; 1151 1152 mat[0] = x1; mat[1] = 0.; mat[2] = 0.; 1153 mat[3] = 0.; mat[4] = x2; mat[5] = 0.; 1154 mat[6] = 0.; mat[7] = 0.; mat[8] = x3; 1155 } 1156 Matrix3D(double x11,double x12,double x13,double x21,double x22,double x23,double x31,double x32,double x33)1157 Matrix3D( 1158 double x11, double x12, double x13, 1159 double x21, double x22, double x23, 1160 double x31, double x32, double x33) 1161 { 1162 rows = 3; cols = 3; 1163 1164 mat[0] = x11; mat[1] = x12; mat[2] = x13; 1165 mat[3] = x21; mat[4] = x22; mat[5] = x23; 1166 mat[6] = x31; mat[7] = x32; mat[8] = x33; 1167 } 1168 Matrix3D(double x11,double x12,double x13,double x14,double x21,double x22,double x23,double x24,double x31,double x32,double x33,double x34)1169 Matrix3D( 1170 double x11, double x12, double x13, double x14, 1171 double x21, double x22, double x23, double x24, 1172 double x31, double x32, double x33, double x34) 1173 { 1174 rows=3; cols=4; 1175 1176 mat[0]=x11; mat[1]=x12; mat[2]=x13; mat[3]=x14; 1177 mat[4]=x21; mat[5]=x22; mat[6]=x23; mat[7]=x24; 1178 mat[8]=x31; mat[9]=x32; mat[10]=x33;mat[11]=x34; 1179 } 1180 1181 //only possible for 1182 Matrix3D(const Matrix& mat); 1183 SetDiag(double x)1184 void SetDiag(double x) 1185 { 1186 rows = 3; cols = 3; 1187 1188 mat[0]=x; mat[1]=0; mat[2]=0; 1189 mat[3]=0; mat[4]=x; mat[5]=0; 1190 mat[6]=0; mat[7]=0; mat[8]=x; 1191 } 1192 1193 //set Matrix as Skew matrix SetSkew(double x1,double x2,double x3)1194 void SetSkew(double x1, double x2, double x3) 1195 { 1196 rows = 3; cols = 3; 1197 1198 mat[0]= 0; mat[1]=-x3; mat[2]= x2; 1199 mat[3]= x3; mat[4]= 0; mat[5]=-x1; 1200 mat[6]=-x2; mat[7]= x1; mat[8]= 0; 1201 } 1202 1203 //set Matrix as Skew matrix SetSkew(const Vector3D & x)1204 void SetSkew(const Vector3D& x) 1205 { 1206 rows = 3; cols = 3; 1207 1208 mat[0]= 0; mat[1]=-x.Z(); mat[2]= x.Y(); 1209 mat[3]= x.Z(); mat[4]= 0; mat[5]=-x.X(); 1210 mat[6]=-x.Y(); mat[7]= x.X(); mat[8]= 0; 1211 } 1212 Set43(double x11,double x12,double x13,double x21,double x22,double x23,double x31,double x32,double x33,double x41,double x42,double x43)1213 void Set43( 1214 double x11, double x12, double x13, 1215 double x21, double x22, double x23, 1216 double x31, double x32, double x33, 1217 double x41, double x42, double x43) 1218 { 1219 rows = 4; cols = 3; 1220 1221 mat[0]=x11; mat[1]=x12; mat[2]=x13; 1222 mat[3]=x21; mat[4]=x22; mat[5]=x23; 1223 mat[6]=x31; mat[7]=x32; mat[8]=x33; 1224 mat[9]=x41;mat[10]=x42;mat[11]=x43; 1225 } 1226 1227 //PG: is this redundant since introduction of class Matrix2D (?) Set22(double x11,double x12,double x21,double x22)1228 void Set22( 1229 double x11, double x12, 1230 double x21, double x22) 1231 { 1232 rows = 2; cols = 2; 1233 1234 mat[0]=x11; mat[1]=x12; 1235 mat[2]=x21; mat[3]=x22; 1236 } 1237 Set(const Vector3D & r1,const Vector3D & r2,const Vector3D & r3)1238 void Set(const Vector3D& r1, const Vector3D& r2, const Vector3D& r3) 1239 { 1240 mat[0] = r1.X(); mat[1] = r2.X(); mat[2] = r3.X(); 1241 mat[3] = r1.Y(); mat[4] = r2.Y(); mat[5] = r3.Y(); 1242 mat[6] = r1.Z(); mat[7] = r2.Z(); mat[8] = r3.Z(); 1243 } 1244 Mises()1245 double Mises() const 1246 { 1247 Matrix3D s; 1248 if (cols == 3) 1249 { 1250 s = *this - (1./3.)*Trace()*Matrix3D(1.); 1251 } 1252 else if (cols == 2) 1253 { 1254 s = *this; 1255 double tr = (1./3.)*Trace(); 1256 s(1,1) -= tr; s(2,2) -= tr; 1257 } 1258 return sqrt(3./2.*(s.InnerProduct(s))); 1259 } 1260 1261 //only for 3x3, fast!!! GetATA2(MatrixXD & m)1262 void GetATA2(MatrixXD& m) 1263 { 1264 for (int i=0; i < 3; i++) 1265 { 1266 for (int j=i; j < 3; j++) 1267 { 1268 m.Get0(i,j) = 0.5*(mat[i]*mat[j] + mat[3+i]*mat[3+j] + mat[2*3+i]*mat[2*3+j]); 1269 } 1270 } 1271 // 0 1 2 1272 // 3 4 5 1273 // 6 7 8 1274 m.Get0(1,0) = m.Get0(0,1); 1275 m.Get0(2,1) = m.Get0(1,2); 1276 m.Get0(2,0) = m.Get0(0,2); 1277 } 1278 GetTp()1279 Matrix3D GetTp() const 1280 { 1281 Matrix3D mt; 1282 mt.SetSize(cols, rows); 1283 1284 for (int i=0; i<rows; i++) 1285 { 1286 for (int j=0; j<cols; j++) 1287 { 1288 mt.Get0(j,i)=Get0(i,j); 1289 } 1290 } 1291 return mt; 1292 } 1293 1294 int GetInverse(Matrix3D& m2); Invert()1295 int Invert() 1296 { 1297 Matrix3D m2; 1298 int rv = GetInverse(m2); 1299 1300 if (rv) 1301 { 1302 *this = m2; 1303 } 1304 1305 return rv; 1306 } 1307 1308 //Arithmetic operations with 1 parameter 1309 Matrix3D& operator+= (const Matrix3D& m) 1310 { 1311 #ifndef __QUICKMATH 1312 release_assert(rows == m.Getrows()); 1313 release_assert(cols == m.Getcols()); 1314 #endif 1315 for (int i=0; i < rows; i++) 1316 { 1317 for (int j=0; j < cols; j++) 1318 { 1319 Get0(i,j) += m.Get0(i,j); 1320 } 1321 } 1322 return *this; 1323 } 1324 1325 Matrix3D& operator-= (const Matrix3D& m) 1326 { 1327 #ifndef __QUICKMATH 1328 release_assert(rows == m.Getrows()); 1329 release_assert(cols == m.Getcols()); 1330 #endif 1331 for (int i=0; i < rows; i++) 1332 { 1333 for (int j=0; j < cols; j++) 1334 { 1335 Get0(i,j) -= m.Get0(i,j); 1336 } 1337 } 1338 return *this; 1339 } 1340 1341 Matrix3D& operator*= (const double& val) 1342 { 1343 for (int i=0; i < rows; i++) 1344 { 1345 for (int j=0; j < cols; j++) 1346 { 1347 Get0(i,j) *= val; 1348 } 1349 } 1350 return *this; 1351 } 1352 1353 //get a orthogonal matrix from vectors v1 and v2 SetOrthogonalBasis(const Vector3D & v1,const Vector3D & v2)1354 void SetOrthogonalBasis(const Vector3D& v1, const Vector3D& v2) 1355 { 1356 Vector3D r1 = v1; 1357 Vector3D r2 = v2; 1358 r1.Normalize(); 1359 1360 double h = r2*r1; 1361 r2 -= h*r1; 1362 r2.Normalize(); 1363 1364 Set(r1,r2,r1.Cross(r2)); 1365 } 1366 1367 1368 //Arithmetic operations with 2 parameters 1369 friend Matrix3D operator+ (const Matrix3D& m1, const Matrix3D& m2); 1370 friend Matrix3D operator- (const Matrix3D& m1, const Matrix3D& m2); 1371 friend Matrix3D operator* (const Matrix3D& m1, const double& val); 1372 friend Matrix3D operator* (const double& val, const Matrix3D& m1); 1373 friend Matrix3D operator* (const Matrix3D& m1, const Matrix3D& m2); 1374 friend Vector3D operator* (const Matrix3D& m, const Vector3D& v); 1375 friend Vector2D operator* (const Matrix3D& m, const Vector2D& v); //this routine is redundant (since introduction of class Matrix2D) 1376 friend Vector3D operator* (const Vector3D& v, const Matrix3D& m); //res=v^T*m =m^T*v 1377 friend Vector3D operator* (const Matrix3D& m, const Vector& v); 1378 1379 friend void Mult(const Matrix3D& m, const Vector3D& v, Vector& res); 1380 friend void Mult(const Matrix3D& m, const Vector3D& v, Vector3D& res); 1381 1382 //transform strain and stress vectors to matrices and vice versa (by PG) 1383 friend void StrainVectorToMatrix3D(Matrix3D& m, const Vector& v); 1384 friend void StressVectorToMatrix3D(Matrix3D& m, const Vector& v); 1385 friend void Matrix3DToStrainVector(Vector& v, const Matrix3D& m); 1386 friend void Matrix3DToStressVector(Vector& v, const Matrix3D& m); 1387 }; 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1403 //++++++++++++++++++++++++ BOX3D +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1404 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1405 1406 class Box3D 1407 { 1408 public: Box3D()1409 Box3D() 1410 { 1411 //set empty box: 1412 pmin = Vector3D(1.,1.,1.); 1413 pmax = Vector3D(-1.,-1.,-1.); 1414 } Box3D(const Vector3D & p1,const Vector3D & p2)1415 Box3D(const Vector3D& p1, const Vector3D& p2) 1416 { 1417 pmin.X() = Minimum(p1.X(),p2.X()); 1418 pmin.Y() = Minimum(p1.Y(),p2.Y()); 1419 pmin.Z() = Minimum(p1.Z(),p2.Z()); 1420 1421 pmax.X() = Maximum(p1.X(),p2.X()); 1422 pmax.Y() = Maximum(p1.Y(),p2.Y()); 1423 pmax.Z() = Maximum(p1.Z(),p2.Z()); 1424 } Box3D(const Box3D & b)1425 Box3D(const Box3D& b) 1426 { 1427 pmin = b.pmin; 1428 pmax = b.pmax; 1429 } Box3D(const Vector3D & c,double r)1430 Box3D(const Vector3D& c, double r) 1431 { 1432 pmin = c; 1433 pmax = c; 1434 Increase(r); 1435 } Empty()1436 int Empty() const 1437 { 1438 if (pmin.X() == 1 && pmax.X() == -1) {return 1;} 1439 return 0; 1440 } Clear()1441 void Clear() 1442 { 1443 //set empty box: 1444 pmin = Vector3D(1.,1.,1.); 1445 pmax = Vector3D(-1.,-1.,-1.); 1446 } Add(const Vector3D & p)1447 void Add(const Vector3D & p) 1448 { 1449 if (Empty()) 1450 { 1451 pmin = p; 1452 pmax = p; 1453 } 1454 else 1455 { 1456 pmin.X() = Minimum(pmin.X(),p.X()); 1457 pmin.Y() = Minimum(pmin.Y(),p.Y()); 1458 pmin.Z() = Minimum(pmin.Z(),p.Z()); 1459 1460 pmax.X() = Maximum(pmax.X(),p.X()); 1461 pmax.Y() = Maximum(pmax.Y(),p.Y()); 1462 pmax.Z() = Maximum(pmax.Z(),p.Z()); 1463 } 1464 } Add(const Box3D & b)1465 void Add(const Box3D& b) 1466 { 1467 if (b.Empty()) return; 1468 1469 if (Empty()) 1470 { 1471 pmin = b.PMin(); 1472 pmax = b.PMax(); 1473 } 1474 else 1475 { 1476 pmin.X() = Minimum(pmin.X(),b.pmin.X()); 1477 pmin.Y() = Minimum(pmin.Y(),b.pmin.Y()); 1478 pmin.Z() = Minimum(pmin.Z(),b.pmin.Z()); 1479 1480 pmax.X() = Maximum(pmax.X(),b.pmax.X()); 1481 pmax.Y() = Maximum(pmax.Y(),b.pmax.Y()); 1482 pmax.Z() = Maximum(pmax.Z(),b.pmax.Z()); 1483 } 1484 } PMin()1485 const Vector3D& PMin() const {return pmin;} PMax()1486 const Vector3D& PMax() const {return pmax;} SizeX()1487 const double SizeX() const {return pmax[0]-pmin[0];} SizeY()1488 const double SizeY() const {return pmax[1]-pmin[1];} SizeZ()1489 const double SizeZ() const {return pmax[2]-pmin[2];} Center()1490 Vector3D Center() const {return 0.5*(pmin+pmax);} Radius()1491 double Radius() const {return 0.5*(pmax-pmin).Norm();} 1492 Increase(double x,double y,double z)1493 void Increase(double x, double y, double z) 1494 { 1495 pmin.X() -= x; 1496 pmin.Y() -= y; 1497 pmin.Z() -= z; 1498 pmax.X() += x; 1499 pmax.Y() += y; 1500 pmax.Z() += z; 1501 } Increase(double x)1502 void Increase(double x) 1503 { 1504 Increase(x,x,x); 1505 } InflateFactor(double x)1506 void InflateFactor(double x) 1507 { 1508 Vector3D pmi = PMin(); 1509 Vector3D pma = PMax(); 1510 Vector3D pc = Center(); 1511 pmin = pc + ( pmi-pc ) * x; 1512 pmax = pc + ( pma-pc ) * x; 1513 } 1514 Intersect(const Box3D & b)1515 int Intersect(const Box3D& b) const 1516 { 1517 if ( pmin[0] > b.pmax[0] || pmax[0] < b.pmin[0] 1518 || pmin[1] > b.pmax[1] || pmax[1] < b.pmin[1] 1519 || pmin[2] > b.pmax[2] || pmax[2] < b.pmin[2]) 1520 return 0; 1521 1522 return 1; 1523 } 1524 1525 /// return 1 if point p in closure IsIn(const Vector3D & p)1526 int IsIn (const Vector3D & p) const 1527 { 1528 if ( pmin[0] <= p[0] && pmax[0] >= p[0] 1529 && pmin[1] <= p[1] && pmax[1] >= p[1] 1530 && pmin[2] <= p[2] && pmax[2] >= p[2]) 1531 return 1; 1532 1533 return 0; 1534 } 1535 1536 private: 1537 Vector3D pmin, pmax; 1538 }; 1539 1540 1541 1542 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1543 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1544 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1545 //Zero-based!!!! 1546 //generate a searchtree with a physical size of Box3D b 1547 //all items must fit into this box and should be equally distributed 1548 //then use AddItems to add items with a bounding box and an identifier 1549 //GetItemsInBox() gives you all identifiers which have a bounding box within the specified box 1550 class SearchTree 1551 { 1552 public: SearchTree()1553 SearchTree():data(0), sx(0), sy(0), sz(0) {}; SearchTree(int sizex,int sizey,int sizez,Box3D b)1554 SearchTree(int sizex, int sizey, int sizez, Box3D b) 1555 { 1556 box = b; 1557 if (box.SizeX()*box.SizeY()*box.SizeZ() == 0) box.Increase(1e-4); 1558 1559 sx = sizex; 1560 sy = sizey; 1561 sz = sizez; 1562 1563 data = new IVector[sx*sy*sz](); 1564 for (int i = 0; i < sx*sy*sz; i++) 1565 { 1566 data[i].Init(); 1567 } 1568 } SearchTree(const SearchTree & tree)1569 SearchTree(const SearchTree& tree) 1570 { 1571 box = tree.box; 1572 1573 sx = tree.sx; 1574 sy = tree.sy; 1575 sz = tree.sz; 1576 1577 data = new IVector[sx*sy*sz](); 1578 for (int i = 0; i < sx*sy*sz; i++) 1579 { 1580 data[i].Init(); 1581 data[i] = tree.data[i]; 1582 } 1583 } 1584 1585 SearchTree& operator=(const SearchTree& tree) 1586 { 1587 if (&tree == this) return *this; 1588 1589 if (data) 1590 { 1591 for (int i = 0; i < sx*sy*sz; i++) 1592 { 1593 data[i].Flush(); 1594 } 1595 delete [] data; 1596 } 1597 1598 box = tree.box; 1599 1600 sx = tree.sx; 1601 sy = tree.sy; 1602 sz = tree.sz; 1603 1604 data = new IVector[sx*sy*sz](); 1605 for (int i = 0; i < sx*sy*sz; i++) 1606 { 1607 data[i] = tree.data[i]; 1608 } 1609 return *this; 1610 } 1611 ~SearchTree()1612 virtual ~SearchTree() 1613 { 1614 if (data) 1615 { 1616 for (int i = 0; i < sx*sy*sz; i++) 1617 { 1618 data[i].Flush(); 1619 } 1620 delete [] data; 1621 } 1622 } 1623 ResetSearchTree(int sizex,int sizey,int sizez,Box3D b)1624 void ResetSearchTree(int sizex, int sizey, int sizez, Box3D b) 1625 { 1626 ClearItems(); //empty all items 1627 box = b; 1628 if (box.SizeX()*box.SizeY()*box.SizeZ() == 0) box.Increase(1e-4); 1629 1630 if (sx != sizex || sy != sizey || sz != sizez) 1631 { 1632 if (data) 1633 { 1634 for (int i = 0; i < sx*sy*sz; i++) 1635 { 1636 data[i].Flush(); 1637 } 1638 delete [] data; 1639 } 1640 1641 sx = sizex; 1642 sy = sizey; 1643 sz = sizez; 1644 1645 data = new IVector[sx*sy*sz](); 1646 for (int i = 0; i < sx*sy*sz; i++) 1647 { 1648 data[i].Init(); 1649 } 1650 } 1651 } 1652 1653 //return 6 indices for box: minx, maxx, miny, maxy, minz, maxz 1654 void GetBoxIndizes(const Box3D& b, int* ind) const; 1655 1656 //return items in box defined by 6 indices: minx, maxx, miny, maxy, minz, maxz 1657 void GetItemsInBox(int* ind, TArray<int>& items) const; 1658 1659 //add items in box defined by 6 indices: minx, maxx, miny, maxy, minz, maxz 1660 //does not reset items list 1661 void AddItemsInBox(int* ind, TArray<int>& items) const; 1662 1663 //get only items in box 1664 void GetItemsInBox(const Box3D& b, TArray<int>& items) const; 1665 1666 //get items in box, do not reset items list 1667 void AddItemsInBox(const Box3D& b, TArray<int>& items) const; 1668 1669 void AddItem(const Box3D& b, int identifier); 1670 1671 //return i-index for a double i-value in Box 1672 int IndX(double x) const; 1673 1674 //return i-index for a double i-value in Box 1675 int IndY(double x) const; 1676 1677 //return i-index for a double i-value in Box 1678 int IndZ(double x) const; 1679 1680 int Index(int x, int y, int z) const; 1681 ClearItems()1682 void ClearItems() 1683 { 1684 for (int ix=0; ix < sx; ix++) 1685 { 1686 for (int iy=0; iy < sy; iy++) 1687 { 1688 for (int iz=0; iz < sz; iz++) 1689 { 1690 data[Index(ix,iy,iz)].SetLen(0); 1691 } 1692 } 1693 } 1694 } 1695 1696 private: 1697 int sx, sy, sz; 1698 Box3D box; 1699 IVector* data; 1700 1701 }; 1702 1703 1704 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1705 //++++++++++++++++++++++++ BOX2D +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1706 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1707 1708 class Box2D 1709 { 1710 public: Box2D()1711 Box2D() 1712 { 1713 //set empty box: 1714 pmin = Vector2D(1.,1.); 1715 pmax = Vector2D(-1.,-1.); 1716 } Box2D(const Vector2D & p1,const Vector2D & p2)1717 Box2D(const Vector2D& p1, const Vector2D& p2) 1718 { 1719 pmin.X() = Minimum(p1.X(),p2.X()); 1720 pmin.Y() = Minimum(p1.Y(),p2.Y()); 1721 1722 pmax.X() = Maximum(p1.X(),p2.X()); 1723 pmax.Y() = Maximum(p1.Y(),p2.Y()); 1724 } Box2D(const Box2D & b)1725 Box2D(const Box2D& b) 1726 { 1727 pmin = b.pmin; 1728 pmax = b.pmax; 1729 } Box2D(const Vector2D & c,double r)1730 Box2D(const Vector2D& c, double r) 1731 { 1732 pmin = c; 1733 pmax = c; 1734 Increase(r); 1735 } Empty()1736 int Empty() const 1737 { 1738 if (pmin.X() > pmax.X()) {return 1;} 1739 return 0; 1740 } Clear()1741 void Clear() 1742 { 1743 pmin = Vector2D(1.,1.); 1744 pmax = Vector2D(-1.,-1.); 1745 } 1746 Add(const Vector2D & p)1747 void Add(const Vector2D & p) 1748 { 1749 if (Empty()) 1750 { 1751 pmin = p; 1752 pmax = p; 1753 } 1754 else 1755 { 1756 pmin.X() = Minimum(pmin.X(),p.X()); 1757 pmin.Y() = Minimum(pmin.Y(),p.Y()); 1758 1759 pmax.X() = Maximum(pmax.X(),p.X()); 1760 pmax.Y() = Maximum(pmax.Y(),p.Y()); 1761 } 1762 } Add(const Box2D & b)1763 void Add(const Box2D& b) 1764 { 1765 if (Empty()) 1766 { 1767 pmin = b.PMin(); 1768 pmax = b.PMax(); 1769 } 1770 else 1771 { 1772 pmin.X() = Minimum(pmin.X(),b.pmin.X()); 1773 pmin.Y() = Minimum(pmin.Y(),b.pmin.Y()); 1774 1775 pmax.X() = Maximum(pmax.X(),b.pmax.X()); 1776 pmax.Y() = Maximum(pmax.Y(),b.pmax.Y()); 1777 } 1778 } PMin()1779 const Vector2D& PMin() const {return pmin;} PMax()1780 const Vector2D& PMax() const {return pmax;} SizeX()1781 const double SizeX() const {return pmax[0]-pmin[0];} SizeY()1782 const double SizeY() const {return pmax[1]-pmin[1];} Center()1783 Vector2D Center() const {return 0.5*(pmin+pmax);} Radius()1784 double Radius() const {return (pmax-pmin).Norm();} Increase(double x)1785 void Increase(double x) 1786 { 1787 Increase(x,x); 1788 } Increase(double x,double y)1789 void Increase(double x, double y) 1790 { 1791 pmin.X() -= x; 1792 pmin.Y() -= y; 1793 pmax.X() += x; 1794 pmax.Y() += y; 1795 } InflateFactor(double x)1796 void InflateFactor(double x) 1797 { 1798 Vector2D pmi = PMin(); 1799 Vector2D pma = PMax(); 1800 Vector2D pc = Center(); 1801 pmin = pc + ( pmi-pc ) * x; 1802 pmax = pc + ( pma-pc ) * x; 1803 } Intersect(const Box2D & b)1804 int Intersect(const Box2D& b) const 1805 { 1806 if ( pmin[0] > b.pmax[0] || pmax[0] < b.pmin[0] 1807 || pmin[1] > b.pmax[1] || pmax[1] < b.pmin[1]) return 0; 1808 1809 return 1; 1810 } 1811 1812 /// return 1 if point p in closure IsIn(const Vector2D & p)1813 int IsIn (const Vector2D & p) const 1814 { 1815 if ( pmin[0] <= p[0] && pmax[0] >= p[0] 1816 && pmin[1] <= p[1] && pmax[1] >= p[1]) 1817 return 1; 1818 1819 return 0; 1820 } 1821 1822 private: 1823 Vector2D pmin, pmax; 1824 }; 1825 1826 1827 1828 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1829 //+++++++++++++++++++++++++++ SEARCHTREE 2D ++++++++++++++++++++++++++++++++++++++++ 1830 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1831 //Zero-based!!!! 1832 //generate a searchtree with a physical size of Box2D b 1833 //all items must fit into this box and should be equally distributed 1834 //then use AddItems to add items with a bounding box and an identifier 1835 //GetItemsInBox() gives you all identifiers which have a bounding box within the specified box 1836 class SearchTree2D 1837 { 1838 public: SearchTree2D()1839 SearchTree2D():data(0), sx(0), sy(0) {}; SearchTree2D(int sizex,int sizey,Box2D b)1840 SearchTree2D(int sizex, int sizey, Box2D b) 1841 { 1842 box = b; 1843 if (box.SizeX()*box.SizeY() == 0) box.Increase(1e-4); 1844 1845 sx = sizex; 1846 sy = sizey; 1847 1848 data = new IVector[sx*sy](); 1849 for (int i = 0; i < sx*sy; i++) 1850 { 1851 data[i].Init(); 1852 } 1853 } SearchTree2D(const SearchTree2D & tree)1854 SearchTree2D(const SearchTree2D& tree) 1855 { 1856 box = tree.box; 1857 1858 sx = tree.sx; 1859 sy = tree.sy; 1860 1861 data = new IVector[sx*sy](); 1862 for (int i = 0; i < sx*sy; i++) 1863 { 1864 data[i].Init(); 1865 data[i] = tree.data[i]; 1866 } 1867 } 1868 1869 SearchTree2D& operator=(const SearchTree2D& tree) 1870 { 1871 if (&tree == this) return *this; 1872 1873 if (data) 1874 { 1875 for (int i = 0; i < sx*sy; i++) 1876 { 1877 data[i].Flush(); 1878 } 1879 delete [] data; 1880 } 1881 1882 box = tree.box; 1883 1884 sx = tree.sx; 1885 sy = tree.sy; 1886 1887 data = new IVector[sx*sy](); 1888 for (int i = 0; i < sx*sy; i++) 1889 { 1890 data[i] = tree.data[i]; 1891 } 1892 return *this; 1893 } 1894 ~SearchTree2D()1895 virtual ~SearchTree2D() 1896 { 1897 if (data) 1898 { 1899 for (int i = 0; i < sx*sy; i++) 1900 { 1901 data[i].Flush(); 1902 } 1903 delete [] data; 1904 } 1905 } 1906 ResetSearchTree(int sizex,int sizey,Box2D b)1907 void ResetSearchTree(int sizex, int sizey, Box2D b) 1908 { 1909 ClearItems(); //empty all items 1910 box = b; 1911 if (box.SizeX()*box.SizeY() == 0) box.Increase(1e-4); 1912 1913 if (sx != sizex || sy != sizey) 1914 { 1915 if (data) 1916 { 1917 for (int i = 0; i < sx*sy; i++) 1918 { 1919 data[i].Flush(); 1920 } 1921 delete [] data; 1922 } 1923 1924 sx = sizex; 1925 sy = sizey; 1926 1927 data = new IVector[sx*sy](); 1928 for (int i = 0; i < sx*sy; i++) 1929 { 1930 data[i].Init(); 1931 } 1932 } 1933 } 1934 GetItemsInBox(const Box2D & b,TArray<int> & items)1935 void GetItemsInBox(const Box2D& b, TArray<int>& items) const 1936 { 1937 items.SetLen(0); 1938 for (int ix=IndX(b.PMin().X()); ix <= IndX(b.PMax().X()); ix++) 1939 { 1940 for (int iy=IndY(b.PMin().Y()); iy <= IndY(b.PMax().Y()); iy++) 1941 { 1942 for (int i = 1; i <= data[Index(ix,iy)].Length(); i++) 1943 items.Add((data[Index(ix,iy)])(i)); 1944 } 1945 } 1946 } 1947 AddItem(const Box2D & b,int identifier)1948 void AddItem(const Box2D& b, int identifier) 1949 { 1950 for (int ix=IndX(b.PMin().X()); ix <= IndX(b.PMax().X()); ix++) 1951 { 1952 for (int iy=IndY(b.PMin().Y()); iy <= IndY(b.PMax().Y()); iy++) 1953 { 1954 data[Index(ix,iy)].Add(identifier); 1955 } 1956 } 1957 } 1958 //return i-index for a double i-value in Box IndX(double x)1959 int IndX(double x) const 1960 { 1961 int i = (int)(((x-box.PMin().X())*sx)/box.SizeX()); 1962 if (i < 0) i = 0; 1963 if (i >= sx) i = sx-1; 1964 return i; 1965 } 1966 //return i-index for a double i-value in Box IndY(double x)1967 int IndY(double x) const 1968 { 1969 int i = (int)(((x-box.PMin().Y())*sy)/box.SizeY()); 1970 if (i < 0) i = 0; 1971 if (i >= sy) i = sy-1; 1972 return i; 1973 } 1974 Index(int x,int y)1975 int Index(int x, int y) const 1976 { 1977 return x+y*sx; 1978 } 1979 ClearItems()1980 void ClearItems() 1981 { 1982 for (int ix=0; ix < sx; ix++) 1983 { 1984 for (int iy=0; iy < sy; iy++) 1985 { 1986 data[Index(ix,iy)].SetLen(0); 1987 } 1988 } 1989 } 1990 1991 private: 1992 int sx, sy; 1993 Box2D box; 1994 IVector* data; 1995 1996 }; 1997 1998 //dont use this function!, use KArdan functions instead! 1999 Matrix3D ComputeRot1Rot2Rot3Matrix(const double phi1, const double phi2, const double phi3); //this is not the same as HOTINT Kardan definition !!!! 2000 void QuaternionsToRot1Rot2Rot3Angles(double beta0, double beta1, double beta2, double beta3, Vector3D& phi); 2001 2002