MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
matvec3.cc File Reference
#include "mbconfig.h"
#include <limits>
#include <cmath>
#include <cfloat>
#include "matvec3.h"
Include dependency graph for matvec3.cc:

Go to the source code of this file.

Namespaces

 CGR_Rot
 

Functions

const doublereal Zero1 (0.)
 
Vec3 operator- (const Vec3 &v)
 
Mat3x3 operator- (const Mat3x3 &m)
 
std::ostream & operator<< (std::ostream &out, const Mat3x3 &m)
 
std::ostream & Write (std::ostream &out, const Mat3x3 &m, const char *s, const char *s2)
 
std::ostream & operator<< (std::ostream &out, const Vec3 &v)
 
std::ostream & Write (std::ostream &out, const Vec3 &v, const char *s)
 
std::ostream & Write (std::ostream &out, const doublereal &d, const char *)
 
Vec3 MatR2gparam (const Mat3x3 &m)
 
Vec3 MatR2LinParam (const Mat3x3 &m)
 
Mat3x3 MatR2vec (unsigned short int ia, const Vec3 &va, unsigned short int ib, const Vec3 &vb)
 
Vec3 MatR2EulerAngles (const Mat3x3 &R)
 
Vec3 MatR2EulerAngles123 (const Mat3x3 &R)
 
Vec3 MatR2EulerAngles313 (const Mat3x3 &R)
 
Vec3 MatR2EulerAngles321 (const Mat3x3 &R)
 
void MatR2EulerParams (const Mat3x3 &R, doublereal &e0, Vec3 &e)
 
Mat3x3 EulerAngles2MatR (const Vec3 &v)
 
Mat3x3 EulerAngles123_2MatR (const Vec3 &v)
 
Mat3x3 EulerAngles313_2MatR (const Vec3 &v)
 
Mat3x3 EulerAngles321_2MatR (const Vec3 &v)
 
Vec3 Unwrap (const Vec3 &vPrev, const Vec3 &v)
 
template<>
bool IsNull (const doublereal &d)
 
template<>
bool IsExactlySame (const doublereal &d1, const doublereal &d2)
 
template<>
bool IsSame (const doublereal &d1, const doublereal &d2, const doublereal &dTol)
 
Vec3 MultRV (const Vec3 &v, const Mat3x3 &R)
 
Mat3x3 MultRM (const Mat3x3 &m, const Mat3x3 &R)
 
Mat3x3 MultMRt (const Mat3x3 &m, const Mat3x3 &R)
 
Mat3x3 MultRMRt (const Mat3x3 &m, const Mat3x3 &R)
 

Variables

const Vec3 Zero3 (0., 0., 0.)
 
const Mat3x3 Eye3 (1., 0., 0., 0., 1., 0., 0., 0., 1.)
 
const Mat3x3 Zero3x3 (0., 0., 0., 0., 0., 0., 0., 0., 0.)
 
const Mat3x3Zero_Manip Mat3x3Zero
 
const Mat3x3DEye_Manip Mat3x3DEye
 
const Mat3x3Diag_Manip Mat3x3Diag
 
const MatCross_Manip MatCross
 
const MatCrossCross_Manip MatCrossCross
 
const Param_Manip CGR_Rot::Param
 
const MatR_Manip CGR_Rot::MatR
 
const MatG_Manip CGR_Rot::MatG
 
const MatGm1_Manip CGR_Rot::MatGm1
 
const char sForm [] = "%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e"
 
const char sDefFill [] = " "
 
const doublereal dRaDegr = 180./M_PI
 

Function Documentation

Mat3x3 EulerAngles123_2MatR ( const Vec3 v)

Definition at line 1014 of file matvec3.cc.

References grad::cos(), and grad::sin().

Referenced by EulerAngles2MatR(), Eu2PhiWrap::Get(), and MBDynParser::GetMatR2vec().

1015 {
1016  doublereal d = v(1);
1017  doublereal dCosAlpha(cos(d));
1018  doublereal dSinAlpha(sin(d));
1019  d = v(2);
1020  doublereal dCosBeta(cos(d));
1021  doublereal dSinBeta(sin(d));
1022  d = v(3);
1023  doublereal dCosGamma(cos(d));
1024  doublereal dSinGamma(sin(d));
1025 
1026  return Mat3x3(
1027  dCosBeta*dCosGamma,
1028  dCosAlpha*dSinGamma + dSinAlpha*dSinBeta*dCosGamma,
1029  dSinAlpha*dSinGamma - dCosAlpha*dSinBeta*dCosGamma,
1030  -dCosBeta*dSinGamma,
1031  dCosAlpha*dCosGamma - dSinAlpha*dSinBeta*dSinGamma,
1032  dSinAlpha*dCosGamma + dCosAlpha*dSinBeta*dSinGamma,
1033  dSinBeta,
1034  -dSinAlpha*dCosBeta,
1035  dCosAlpha*dCosBeta);
1036 };
GradientExpression< UnaryExpr< FuncSin, Expr > > sin(const GradientExpression< Expr > &u)
Definition: gradient.h:2977
GradientExpression< UnaryExpr< FuncCos, Expr > > cos(const GradientExpression< Expr > &u)
Definition: gradient.h:2978
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

Mat3x3 EulerAngles2MatR ( const Vec3 v)

Definition at line 1008 of file matvec3.cc.

References EulerAngles123_2MatR().

Referenced by main().

1009 {
1010  return EulerAngles123_2MatR(v);
1011 }
Mat3x3 EulerAngles123_2MatR(const Vec3 &v)
Definition: matvec3.cc:1014

Here is the call graph for this function:

Mat3x3 EulerAngles313_2MatR ( const Vec3 v)

Definition at line 1039 of file matvec3.cc.

References grad::cos(), and grad::sin().

Referenced by Eu2PhiWrap::Get(), and MBDynParser::GetMatR2vec().

1040 {
1041  doublereal d = v(1);
1042  doublereal dCosAlpha(cos(d));
1043  doublereal dSinAlpha(sin(d));
1044  d = v(2);
1045  doublereal dCosBeta(cos(d));
1046  doublereal dSinBeta(sin(d));
1047  d = v(3);
1048  doublereal dCosGamma(cos(d));
1049  doublereal dSinGamma(sin(d));
1050 
1051  return Mat3x3(
1052  dCosAlpha*dCosGamma - dSinAlpha*dCosBeta*dSinGamma,
1053  dSinAlpha*dCosGamma + dCosAlpha*dCosBeta*dSinGamma,
1054  dSinBeta*dSinGamma,
1055  -dCosAlpha*dSinGamma - dSinAlpha*dCosBeta*dCosGamma,
1056  -dSinAlpha*dSinGamma + dCosAlpha*dCosBeta*dCosGamma,
1057  dSinBeta*dCosGamma,
1058  dSinAlpha*dSinBeta,
1059  -dCosAlpha*dSinBeta,
1060  dCosBeta);
1061 }
GradientExpression< UnaryExpr< FuncSin, Expr > > sin(const GradientExpression< Expr > &u)
Definition: gradient.h:2977
GradientExpression< UnaryExpr< FuncCos, Expr > > cos(const GradientExpression< Expr > &u)
Definition: gradient.h:2978
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

Mat3x3 EulerAngles321_2MatR ( const Vec3 v)

Definition at line 1064 of file matvec3.cc.

References grad::cos(), and grad::sin().

Referenced by Eu2PhiWrap::Get(), and MBDynParser::GetMatR2vec().

1065 {
1066  doublereal d = v(1);
1067  doublereal dCosAlpha(cos(d));
1068  doublereal dSinAlpha(sin(d));
1069  d = v(2);
1070  doublereal dCosBeta(cos(d));
1071  doublereal dSinBeta(sin(d));
1072  d = v(3);
1073  doublereal dCosGamma(cos(d));
1074  doublereal dSinGamma(sin(d));
1075 
1076  return Mat3x3(
1077  dCosAlpha*dCosBeta,
1078  dSinAlpha*dCosBeta,
1079  -dSinBeta,
1080  -dSinAlpha*dCosGamma + dCosAlpha*dSinBeta*dSinGamma,
1081  dCosAlpha*dCosGamma + dSinAlpha*dSinBeta*dSinGamma,
1082  dCosBeta*dSinGamma,
1083  dSinAlpha*dSinGamma + dCosAlpha*dSinBeta*dCosGamma,
1084  -dCosAlpha*dSinGamma + dSinAlpha*dSinBeta*dCosGamma,
1085  dCosBeta*dCosGamma);
1086 }
GradientExpression< UnaryExpr< FuncSin, Expr > > sin(const GradientExpression< Expr > &u)
Definition: gradient.h:2977
GradientExpression< UnaryExpr< FuncCos, Expr > > cos(const GradientExpression< Expr > &u)
Definition: gradient.h:2978
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

template<>
bool IsExactlySame ( const doublereal d1,
const doublereal d2 
)

Definition at line 1131 of file matvec3.cc.

Referenced by ReadStructExtForce().

1132 {
1133  return d1 == d2;
1134 }
template<>
bool IsNull ( const doublereal d)

Definition at line 1124 of file matvec3.cc.

Referenced by LinearViscoElasticGenericCLR< T, Tder >::Read(), and LTVViscoElasticGenericCLR< T, Tder >::Read().

1125 {
1126  return d == 0.;
1127 }
template<>
bool IsSame ( const doublereal d1,
const doublereal d2,
const doublereal dTol 
)

Definition at line 1138 of file matvec3.cc.

References grad::fabs().

Referenced by ReferenceFrame::ReferenceFrame().

1139 {
1140  return fabs(d1 - d2) <= dTol;
1141 }
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
Definition: gradient.h:2973

Here is the call graph for this function:

Vec3 MatR2EulerAngles ( const Mat3x3 R)

Definition at line 887 of file matvec3.cc.

References MatR2EulerAngles123().

Referenced by Brake::AfterConvergence(), main(), DriveHingeJoint::Output(), UniversalHingeJoint::Output(), Brake::Output(), PinJoint::Output(), UniversalPinJoint::Output(), PlanePinJoint::Output(), Brake::SetValue(), and AxialRotationJoint::SetValue().

888 {
889  return MatR2EulerAngles123(R);
890 }
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
Definition: matvec3.cc:893

Here is the call graph for this function:

Vec3 MatR2EulerAngles123 ( const Mat3x3 R)

Definition at line 893 of file matvec3.cc.

References grad::atan2(), grad::cos(), R, and grad::sin().

Referenced by StructNode::dGetPrivData(), MatR2EulerAngles(), ReferenceFrame::Output(), GimbalRotationJoint::Output(), SphericalHingeJoint::Output(), DeformableHingeJoint::Output(), DeformableJoint::Output(), StructExtForce::Output(), PlaneHingeJoint::Output(), UniversalRotationJoint::Output(), Beam2::Output(), PlaneRotationJoint::Output(), Beam::Output(), AxialRotationJoint::Output(), StructNode::Output(), Aerodynamic2DElem< iNN >::Output_int(), DeformableHingeJoint::OutputInv(), ReadStructExtForce(), ReadStructMappingExtForce(), ReadStructNode(), StructExtForce::SendToFileDes(), StructMappingExtForce::SendToFileDes(), StructMembraneMappingExtForce::SendToFileDes(), StructExtForce::SendToStream(), StructMappingExtForce::SendToStream(), and StructMembraneMappingExtForce::SendToStream().

894 {
895  doublereal dAlpha = atan2(-R(2, 3), R(3, 3));
896  doublereal dCosAlpha = cos(dAlpha);
897  doublereal dSinAlpha = sin(dAlpha);
898 
899  return Vec3(dAlpha,
900  atan2(R(1, 3),
901  dCosAlpha*R(3, 3) - dSinAlpha*R(2, 3)),
902  atan2(dCosAlpha*R(2, 1) + dSinAlpha*R(3, 1),
903  dCosAlpha*R(2, 2) + dSinAlpha*R(3, 2)));
904 }
Definition: matvec3.h:98
GradientExpression< UnaryExpr< FuncSin, Expr > > sin(const GradientExpression< Expr > &u)
Definition: gradient.h:2977
GradientExpression< UnaryExpr< FuncCos, Expr > > cos(const GradientExpression< Expr > &u)
Definition: gradient.h:2978
GradientExpression< BinaryExpr< FuncAtan2, LhsExpr, RhsExpr > > atan2(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2962
double doublereal
Definition: colamd.c:52
Mat3x3 R

Here is the call graph for this function:

Vec3 MatR2EulerAngles313 ( const Mat3x3 R)

Definition at line 927 of file matvec3.cc.

References grad::atan2(), grad::cos(), R, and grad::sin().

Referenced by StructNode::dGetPrivData(), ReferenceFrame::Output(), GimbalRotationJoint::Output(), SphericalHingeJoint::Output(), DeformableHingeJoint::Output(), DeformableJoint::Output(), PlaneHingeJoint::Output(), UniversalRotationJoint::Output(), Beam2::Output(), PlaneRotationJoint::Output(), Beam::Output(), AxialRotationJoint::Output(), StructNode::Output(), Aerodynamic2DElem< iNN >::Output_int(), DeformableHingeJoint::OutputInv(), and ReadStructNode().

928 {
929  doublereal dAlpha = atan2(R(1, 3), -R(2, 3));
930  doublereal dCosAlpha = cos(dAlpha);
931  doublereal dSinAlpha = sin(dAlpha);
932 
933  return Vec3(dAlpha,
934  atan2(dSinAlpha*R(1, 3) - dCosAlpha*R(2, 3),
935  R(3, 3)),
936  atan2(-dCosAlpha*R(1, 2) - dSinAlpha*R(2, 2),
937  dCosAlpha*R(1, 1) + dSinAlpha*R(2, 1)));
938 }
Definition: matvec3.h:98
GradientExpression< UnaryExpr< FuncSin, Expr > > sin(const GradientExpression< Expr > &u)
Definition: gradient.h:2977
GradientExpression< UnaryExpr< FuncCos, Expr > > cos(const GradientExpression< Expr > &u)
Definition: gradient.h:2978
GradientExpression< BinaryExpr< FuncAtan2, LhsExpr, RhsExpr > > atan2(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2962
double doublereal
Definition: colamd.c:52
Mat3x3 R

Here is the call graph for this function:

Vec3 MatR2EulerAngles321 ( const Mat3x3 R)

Definition at line 941 of file matvec3.cc.

References grad::atan2(), grad::cos(), R, and grad::sin().

Referenced by StructNode::dGetPrivData(), ReferenceFrame::Output(), GimbalRotationJoint::Output(), SphericalHingeJoint::Output(), DeformableHingeJoint::Output(), DeformableJoint::Output(), PlaneHingeJoint::Output(), UniversalRotationJoint::Output(), Beam2::Output(), PlaneRotationJoint::Output(), Beam::Output(), AxialRotationJoint::Output(), StructNode::Output(), Aerodynamic2DElem< iNN >::Output_int(), DeformableHingeJoint::OutputInv(), and ReadStructNode().

942 {
943  doublereal dAlpha = atan2(R(2, 1), R(1, 1));
944  doublereal dCosAlpha = cos(dAlpha);
945  doublereal dSinAlpha = sin(dAlpha);
946 
947  return Vec3(dAlpha,
948  atan2(-R(3, 1),
949  dCosAlpha*R(1, 1) + dSinAlpha*R(2, 1)),
950  atan2(dSinAlpha*R(1, 3) - dCosAlpha*R(2, 3),
951  -dSinAlpha*R(1, 2) + dCosAlpha*R(2, 2)));
952 }
Definition: matvec3.h:98
GradientExpression< UnaryExpr< FuncSin, Expr > > sin(const GradientExpression< Expr > &u)
Definition: gradient.h:2977
GradientExpression< UnaryExpr< FuncCos, Expr > > cos(const GradientExpression< Expr > &u)
Definition: gradient.h:2978
GradientExpression< BinaryExpr< FuncAtan2, LhsExpr, RhsExpr > > atan2(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2962
double doublereal
Definition: colamd.c:52
Mat3x3 R

Here is the call graph for this function:

void MatR2EulerParams ( const Mat3x3 R,
doublereal e0,
Vec3 e 
)

Definition at line 954 of file matvec3.cc.

References Mat3x3::dGet(), grad::fabs(), grad::sqrt(), and Mat3x3::Tr().

Referenced by StructNode::dGetPrivData(), and main().

955 {
956  doublereal t = R.Tr();
957  doublereal T[4];
958  T[0] = 1. + t;
959  T[1] = 1. - t + 2.*R.dGet(1, 1);
960  T[2] = 1. - t + 2.*R.dGet(2, 2);
961  T[3] = 1. - t + 2.*R.dGet(3, 3);
962 
963  int k = 0;
964  for (int i = 1; i <= 3; i++) {
965  if (fabs(T[i]) >= fabs(T[k])) {
966  k = i;
967  }
968  }
969 
970  switch (k) {
971  case 0:
972  e0 = .5*sqrt(T[0]);
973  e = Vec3(R.dGet(3, 2)-R.dGet(2, 3),
974  R.dGet(1, 3)-R.dGet(3, 1),
975  R.dGet(2, 1)-R.dGet(1, 2))/(4.*e0);
976  break;
977 
978  case 1: {
979  doublereal e1 = .5*sqrt(T[1]);
980  e0 = (R.dGet(3, 2) - R.dGet(2, 3))/(4.*e1);
981  e = Vec3(T[1],
982  R.dGet(2, 1)+R.dGet(1, 2),
983  R.dGet(3, 1)+R.dGet(1, 3))/(4.*e1);
984  break;
985  }
986 
987  case 2: {
988  doublereal e2 = .5*sqrt(T[2]);
989  e0 = (R.dGet(1, 3) - R.dGet(3, 1))/(4.*e2);
990  e = Vec3(R.dGet(1, 2)+R.dGet(2, 1),
991  T[2],
992  R.dGet(3, 2)+R.dGet(2, 3))/(4.*e2);
993  break;
994  }
995 
996  case 3: {
997  doublereal e3 = .5*sqrt(T[3]);
998  e0 = (R.dGet(2, 1) - R.dGet(1, 2))/(4.*e3);
999  e = Vec3(R.dGet(1, 3)+R.dGet(3, 1),
1000  R.dGet(2, 3)+R.dGet(3, 2),
1001  T[3])/(4.*e3);
1002  break;
1003  }
1004  }
1005 }
const doublereal & dGet(unsigned short int iRow, unsigned short int iCol) const
Definition: matvec3.h:770
Definition: matvec3.h:98
doublereal Tr(void) const
Definition: matvec3.h:1361
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
Definition: gradient.h:2973
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
Definition: gradient.h:2974
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

Vec3 MatR2gparam ( const Mat3x3 m)

Definition at line 756 of file matvec3.cc.

References Mat3x3::Ax(), grad::fabs(), MBDYN_EXCEPT_ARGS, and Mat3x3::Trace().

Referenced by HBeam::Omega0().

757 {
758  /* test di singolarita' */
759  doublereal d = 1. + m.Trace();
760 
761  if (fabs(d) < std::numeric_limits<doublereal>::epsilon()) {
762  silent_cerr("MatR2gparam(): divide by zero, "
763  "probably due to singularity in rotation parameters" << std::endl);
765  }
766 
767  return m.Ax()*(4./d);
768 }
Vec3 Ax(void) const
Definition: matvec3.h:827
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
Definition: gradient.h:2973
doublereal Trace(void) const
Definition: matvec3.h:836
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

Vec3 MatR2LinParam ( const Mat3x3 m)

Definition at line 772 of file matvec3.cc.

References Mat3x3::Ax().

Referenced by ModalMappingExt::Send().

773 {
774  return m.Ax();
775 }
Vec3 Ax(void) const
Definition: matvec3.h:827

Here is the call graph for this function:

Mat3x3 MatR2vec ( unsigned short int  ia,
const Vec3 va,
unsigned short int  ib,
const Vec3 vb 
)

Definition at line 779 of file matvec3.cc.

References ASSERT, Vec3::Cross(), DEBUGCOUT, Vec3::Dot(), MBDYN_EXCEPT_ARGS, Vec3::Norm(), grad::sqrt(), and Zero3x3.

Referenced by MBDynParser::GetMatR2vec(), and main().

781 {
782  ASSERT(ia >= 1 && ia <= 3);
783  ASSERT(ib >= 1 && ib <= 3);
784 
785  Vec3 r[3];
786 
787  DEBUGCOUT("MatR2vec: ia = " << ia << " (" << va << "),"
788  << " ib = " << ib << " (" << vb << ")" << std::endl);
789 
790  if (ia < 1 || ia > 3) {
791  silent_cerr("MatR2vec: first index is illegal"
792  << std::endl);
794  }
795 
796  int i1 = ia-1;
797  int i2 = ia%3;
798  int i3 = (ia+1)%3;
799 
800  if (ib == (ia%3)+1) {
801  doublereal d = va.Norm();
802  if (d <= std::numeric_limits<doublereal>::epsilon()) {
803  silent_cerr("MatR2vec: first vector must be non-null" << std::endl );
805  }
806  r[i1] = va/d;
807  d = vb.Norm();
808  if (d <= std::numeric_limits<doublereal>::epsilon()) {
809  silent_cerr("MatR2vec: second vector must be non-null" << std::endl );
811  }
812  r[i3] = r[i1].Cross(vb);
813  d = r[i3].Dot();
814  if (d <= std::numeric_limits<doublereal>::epsilon()) {
815  silent_cerr("MatR2vec: vectors must be distinct"
816  << std::endl);
818  }
819  d = sqrt(d);
820  r[i3] /= d;
821  r[i2] = r[i3].Cross(r[i1]);
822 
823  DEBUGCOUT("R = " << Mat3x3(r[0], r[1], r[2]) << std::endl);
824 
825  return Mat3x3(r[0], r[1], r[2]);
826  } else if (ib == ((ia+1)%3+1)) {
827  doublereal d = va.Norm();
828  if (d <= std::numeric_limits<doublereal>::epsilon()) {
829  silent_cerr("MatR2vec: first vector must be non-null" << std::endl );
831  }
832  r[i1] = va/d;
833  d = vb.Norm();
834  if (d <= std::numeric_limits<doublereal>::epsilon()) {
835  silent_cerr("MatR2vec: second vector must be non-null" << std::endl );
837  }
838  r[i2] = vb.Cross(r[i1]);
839  d = r[i2].Dot();
840  if (d <= std::numeric_limits<doublereal>::epsilon()) {
841  silent_cerr("MatR2vec: vectors must be distinct"
842  << std::endl);
844  }
845  d = sqrt(d);
846  r[i2] /= d;
847  r[i3] = r[i1].Cross(r[i2]);
848 
849  DEBUGCOUT("R = " << Mat3x3(r[0], r[1], r[2]) << std::endl);
850 
851  return Mat3x3(r[0], r[1], r[2]);
852  } else {
853  silent_cerr("MatR2vec: second index is illegal" << std::endl);
855  }
856 
857  return ::Zero3x3; // phony call, not reachable
858 }
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Definition: matvec3.h:98
doublereal Norm(void) const
Definition: matvec3.h:263
doublereal Dot(const Vec3 &v) const
Definition: matvec3.h:243
const Mat3x3 Zero3x3(0., 0., 0., 0., 0., 0., 0., 0., 0.)
#define DEBUGCOUT(msg)
Definition: myassert.h:232
#define ASSERT(expression)
Definition: colamd.c:977
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
Definition: gradient.h:2974
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

Mat3x3 MultMRt ( const Mat3x3 m,
const Mat3x3 R 
)

Definition at line 1156 of file matvec3.cc.

References Mat3x3::MulMT().

1157 {
1158  return m.MulMT(R);
1159 }
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444

Here is the call graph for this function:

Mat3x3 MultRM ( const Mat3x3 m,
const Mat3x3 R 
)

Definition at line 1150 of file matvec3.cc.

1151 {
1152  return R*m;
1153 }
Vec3 operator- ( const Vec3 v)

Definition at line 651 of file matvec3.cc.

References Vec3::pdVec, V1, V2, and V3.

652 {
653  return Vec3(-v.pdVec[V1],
654  -v.pdVec[V2],
655  -v.pdVec[V3]);
656 }
Definition: matvec3.h:98
Definition: matvec3.h:50
doublereal pdVec[3]
Definition: matvec3.h:109
Definition: matvec3.h:51
Definition: matvec3.h:49
Mat3x3 operator- ( const Mat3x3 m)

Definition at line 659 of file matvec3.cc.

References M11, M12, M13, M21, M22, M23, M31, M32, M33, and Mat3x3::pGetMat().

660 {
661  const doublereal* pdMat = m.pGetMat();
662  return Mat3x3(-pdMat[M11],
663  -pdMat[M21],
664  -pdMat[M31],
665  -pdMat[M12],
666  -pdMat[M22],
667  -pdMat[M32],
668  -pdMat[M13],
669  -pdMat[M23],
670  -pdMat[M33]);
671 }
Definition: matvec3.h:59
Definition: matvec3.h:58
Definition: matvec3.h:55
Definition: matvec3.h:56
Definition: matvec3.h:63
Definition: matvec3.h:62
Definition: matvec3.h:61
Definition: matvec3.h:57
const doublereal * pGetMat(void) const
Definition: matvec3.h:743
Definition: matvec3.h:60
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

std::ostream& operator<< ( std::ostream &  out,
const Mat3x3 m 
)

Definition at line 680 of file matvec3.cc.

References M11, M12, M13, M21, M22, M23, M31, M32, M33, and Mat3x3::pGetMat().

681 {
682  const doublereal* pd = m.pGetMat();
683 
684  out
685  << pd[M11] << sDefFill << pd[M12] << sDefFill << pd[M13] << sDefFill
686  << pd[M21] << sDefFill << pd[M22] << sDefFill << pd[M23] << sDefFill
687  << pd[M31] << sDefFill << pd[M32] << sDefFill << pd[M33];
688 
689  return out;
690 }
Definition: matvec3.h:59
Definition: matvec3.h:58
Definition: matvec3.h:55
Definition: matvec3.h:56
const char sDefFill[]
Definition: matvec3.cc:675
Definition: matvec3.h:63
Definition: matvec3.h:62
Definition: matvec3.h:61
Definition: matvec3.h:57
const doublereal * pGetMat(void) const
Definition: matvec3.h:743
Definition: matvec3.h:60
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

std::ostream& operator<< ( std::ostream &  out,
const Vec3 v 
)

Definition at line 703 of file matvec3.cc.

References Vec3::pGetVec().

704 {
705  const doublereal* pd = v.pGetVec();
706 
707  out << pd[0] << sDefFill << pd[1] << sDefFill << pd[2];
708 
709  return out;
710 }
const char sDefFill[]
Definition: matvec3.cc:675
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

Vec3 Unwrap ( const Vec3 vPrev,
const Vec3 v 
)

Definition at line 1089 of file matvec3.cc.

References M_PI, and Vec3::Norm().

Referenced by TotalEquation::AfterConvergence(), TotalJoint::AfterConvergence(), TotalReaction::AfterConvergence(), TotalPinJoint::AfterConvergence(), TotalEquation::dGetPrivData(), TotalJoint::dGetPrivData(), TotalReaction::dGetPrivData(), and TotalPinJoint::dGetPrivData().

1090 {
1091  doublereal dTheta = v.Norm();
1092  if (dTheta > 0.) {
1093  doublereal dThetaPrev = vPrev.Norm();
1094  if (dThetaPrev > std::numeric_limits<doublereal>::epsilon()) {
1095  bool b(false);
1096 
1097  if (vPrev*v < 0) {
1098  dTheta = -dTheta;
1099  }
1100 
1101  doublereal dThetaOld = dTheta;
1102 
1103  while (dTheta - dThetaPrev > M_PI) {
1104  dTheta -= 2.*M_PI;
1105  b = true;
1106  }
1107 
1108  while (dThetaPrev - dTheta > M_PI) {
1109  dTheta += 2.*M_PI;
1110  b = true;
1111  }
1112 
1113  if (b) {
1114  return v*(dTheta/dThetaOld);
1115  }
1116  }
1117  }
1118 
1119  return v;
1120 }
#define M_PI
Definition: gradienttest.cc:67
doublereal Norm(void) const
Definition: matvec3.h:263
double doublereal
Definition: colamd.c:52

Here is the call graph for this function:

std::ostream& Write ( std::ostream &  out,
const Mat3x3 m,
const char *  s,
const char *  s2 
)

Definition at line 694 of file matvec3.cc.

References Mat3x3::Write().

695 {
696  return m.Write(out, s, s2);
697 }
std::ostream & Write(std::ostream &out, const char *sFill=" ", const char *sFill2=NULL) const
Definition: matvec3.cc:722

Here is the call graph for this function:

std::ostream& Write ( std::ostream &  out,
const Vec3 v,
const char *  s 
)

Definition at line 714 of file matvec3.cc.

References Vec3::Write().

715 {
716  return v.Write(out, s);
717 }
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738

Here is the call graph for this function:

std::ostream& Write ( std::ostream &  out,
const doublereal d,
const char *   
)

Definition at line 747 of file matvec3.cc.

748 {
749  return out << d;
750 }
const doublereal Zero1 ( 0.  )

Variable Documentation

const Mat3x3Diag_Manip Mat3x3Diag

Definition at line 638 of file matvec3.cc.

const Mat3x3Zero_Manip Mat3x3Zero

Definition at line 636 of file matvec3.cc.

const MatCross_Manip MatCross

Definition at line 639 of file matvec3.cc.

Referenced by HydrodynamicPlainBearing::AssJac(), InlineFriction::AssJac(), UniversalHingeJoint::AssJac(), Actuator::AssJac(), asynchronous_machine::AssJac(), RodBezier::AssJac(), Motor::AssJac(), PlaneHingeJoint::AssJac(), BeamSliderJoint::AssJac(), TotalEquation::AssJac(), TotalJoint::AssJac(), UniversalPinJoint::AssJac(), AerodynamicBody::AssJac(), Modal::AssJac(), RodWithOffset::AssJac(), FollowerForce::AssJac(), TotalReaction::AssJac(), Shell4EAS::AssJac(), ModuleIMUConstraint::AssJac(), AerodynamicBeam::AssJac(), FollowerCouple::AssJac(), AxialRotationJoint::AssJac(), AerodynamicBeam2::AssJac(), TotalForce::AssJac(), LoadIncForce::AssJac(), FollowerInternalForce::AssJac(), FollowerInternalCouple::AssJac(), GimbalRotationJoint::AssMat(), DriveDisplacementJoint::AssMat(), DriveHingeJoint::AssMat(), DriveDisplacementPinJoint::AssMat(), DeformableJoint::AssMatCommon(), DeformableJoint::AssMatElastic(), DeformableDispJoint::AssMatF(), DeformableDispJoint::AssMatFDE(), DeformableDispJoint::AssMatFDEPrime(), DeformableAxialJoint::AssMatM(), DeformableHingeJoint::AssMatM(), DeformableAxialJoint::AssMatMDEPrime(), DeformableHingeJoint::AssMatMDEPrime(), ViscousBody::AssMats(), DynamicVariableBody::AssMats(), DynamicBody::AssMats(), ModalBody::AssMats(), Mass::AssMatsRBK_int(), VariableBody::AssMatsRBK_int(), Body::AssMatsRBK_int(), DeformableJoint::AssMatViscous(), Shell4EAS::AssRes(), Shell4EASANS::AssRes(), PiezoActuatorBeam::AssStiffnessMat(), PiezoActuatorBeam2::AssStiffnessMat(), Beam2::AssStiffnessMat(), HBeam::AssStiffnessMat(), PiezoActuatorVEBeam2::AssStiffnessMat(), PiezoActuatorVEBeam::AssStiffnessMat(), Beam::AssStiffnessMat(), ViscoElasticBeam2::AssStiffnessMat(), ViscoElasticBeam::AssStiffnessMat(), ComputeFullInterpolation(), VecExp::Cross(), RotManip::Elle(), Modal::GetCurrFEMNodesVelocity(), InlineFriction::InitialAssJac(), InLineJoint::InitialAssJac(), UniversalHingeJoint::InitialAssJac(), InPlaneJoint::InitialAssJac(), PrismaticJoint::InitialAssJac(), SphericalHingeJoint::InitialAssJac(), RodBezier::InitialAssJac(), DriveDisplacementJoint::InitialAssJac(), DriveHingeJoint::InitialAssJac(), PlaneHingeJoint::InitialAssJac(), DynamicVariableBody::InitialAssJac(), InLineWithOffsetJoint::InitialAssJac(), DistanceJointWithOffset::InitialAssJac(), UniversalRotationJoint::InitialAssJac(), PinJoint::InitialAssJac(), InPlaneWithOffsetJoint::InitialAssJac(), TotalJoint::InitialAssJac(), AbsoluteForce::InitialAssJac(), DriveDisplacementPinJoint::InitialAssJac(), UniversalPinJoint::InitialAssJac(), PlaneRotationJoint::InitialAssJac(), Modal::InitialAssJac(), RodWithOffset::InitialAssJac(), FollowerForce::InitialAssJac(), ViscousAxialJoint::InitialAssJac(), TotalReaction::InitialAssJac(), DynamicBody::InitialAssJac(), FollowerCouple::InitialAssJac(), ViscousHingeJoint::InitialAssJac(), ViscousDispJoint::InitialAssJac(), ViscoElasticAxialJoint::InitialAssJac(), AxialRotationJoint::InitialAssJac(), AbsoluteInternalForce::InitialAssJac(), TotalForce::InitialAssJac(), ViscoElasticDispJoint::InitialAssJac(), PlanePinJoint::InitialAssJac(), FollowerInternalForce::InitialAssJac(), ViscoElasticHingeJoint::InitialAssJac(), FollowerInternalCouple::InitialAssJac(), Modal::InitialAssRes(), MatExp::MatExp(), MultMatVCross(), MultMatVCrossT(), ReadModal(), Mat3x3::Skew(), testMatVec3(), and InvAngularConstitutiveLaw::Update().

const MatCrossCross_Manip MatCrossCross

Definition at line 640 of file matvec3.cc.

Referenced by VariableBody::AfterPredict(), InLineJoint::AssJac(), InlineFriction::AssJac(), UniversalHingeJoint::AssJac(), PrismaticJoint::AssJac(), SphericalHingeJoint::AssJac(), InPlaneJoint::AssJac(), RodBezier::AssJac(), Brake::AssJac(), PlaneHingeJoint::AssJac(), BeamSliderJoint::AssJac(), InLineWithOffsetJoint::AssJac(), TotalEquation::AssJac(), TotalJoint::AssJac(), DistanceJointWithOffset::AssJac(), UniversalRotationJoint::AssJac(), PinJoint::AssJac(), InPlaneWithOffsetJoint::AssJac(), ViscoElasticRod::AssJac(), AbsoluteForce::AssJac(), UniversalPinJoint::AssJac(), PlaneRotationJoint::AssJac(), Modal::AssJac(), RodWithOffset::AssJac(), TotalReaction::AssJac(), TotalPinJoint::AssJac(), AerodynamicBeam::AssJac(), AxialRotationJoint::AssJac(), AerodynamicBeam2::AssJac(), AbsoluteInternalForce::AssJac(), TotalForce::AssJac(), LoadIncForce::AssJac(), FollowerInternalForce::AssJac(), PlanePinJoint::AssJac(), DriveDisplacementJoint::AssMat(), Rod::AssMat(), DriveDisplacementPinJoint::AssMat(), DeformableJoint::AssMatCommon(), DeformableDispJoint::AssMatF(), DeformableDispJoint::AssMatFDEPrime(), DynamicVariableBody::AssMats(), StaticVariableBody::AssMats(), DynamicBody::AssMats(), ModalBody::AssMats(), StaticBody::AssMats(), Mass::AssMatsRBK_int(), VariableBody::AssMatsRBK_int(), Body::AssMatsRBK_int(), DeformableJoint::AssMatViscous(), DynamicVariableBody::AssRes(), StaticVariableBody::AssRes(), Beam2::AssStiffnessMat(), Beam::AssStiffnessMat(), ViscoElasticBeam2::AssStiffnessMat(), ViscoElasticBeam::AssStiffnessMat(), CenterOfMass::Collect_int(), Inertia::Collect_int(), AutomaticStructElem::ComputeAccelerations(), Body::dGetPrivData(), RotManip::DRot(), RotManip::DRot_I(), RotManip::DRot_IT(), RotManip::Elle(), Mass::GetJ_int(), VariableBody::GetJ_int(), Modal::GetJ_int(), Body::GetJ_int(), InlineFriction::InitialAssJac(), InLineJoint::InitialAssJac(), UniversalHingeJoint::InitialAssJac(), InPlaneJoint::InitialAssJac(), PrismaticJoint::InitialAssJac(), SphericalHingeJoint::InitialAssJac(), RodBezier::InitialAssJac(), DriveDisplacementJoint::InitialAssJac(), PlaneHingeJoint::InitialAssJac(), DynamicVariableBody::InitialAssJac(), InLineWithOffsetJoint::InitialAssJac(), DistanceJointWithOffset::InitialAssJac(), UniversalRotationJoint::InitialAssJac(), PinJoint::InitialAssJac(), InPlaneWithOffsetJoint::InitialAssJac(), TotalJoint::InitialAssJac(), ViscoElasticRod::InitialAssJac(), AbsoluteForce::InitialAssJac(), DriveDisplacementPinJoint::InitialAssJac(), UniversalPinJoint::InitialAssJac(), PlaneRotationJoint::InitialAssJac(), Modal::InitialAssJac(), RodWithOffset::InitialAssJac(), FollowerForce::InitialAssJac(), TotalReaction::InitialAssJac(), DynamicBody::InitialAssJac(), TotalPinJoint::InitialAssJac(), FollowerCouple::InitialAssJac(), AxialRotationJoint::InitialAssJac(), AbsoluteInternalForce::InitialAssJac(), TotalForce::InitialAssJac(), PlanePinJoint::InitialAssJac(), FollowerInternalForce::InitialAssJac(), FollowerInternalCouple::InitialAssJac(), DynamicVariableBody::InitialAssRes(), MatExp::MatExp(), Inertia::Output_int(), ReadBody(), ReadModal(), Body::Restart(), RotManip::Rot(), RotManip::RotAndDRot(), RotManip::RotAndDRot_IT(), DynamicVariableBody::SetValue(), and testMatVec3().

const char sDefFill[] = " "

Definition at line 675 of file matvec3.cc.

const char sForm[] = "%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e%15.6e"

Definition at line 674 of file matvec3.cc.

const Vec3 Zero3(0., 0., 0.)

Referenced by AeroDynModule::AeroDynModule(), ModuleNonsmoothNode::AfterConvergence(), ViscousHingeJoint::AfterConvergence(), ViscousDispJoint::AfterConvergence(), CyclocopterUniform1D::AfterConvergence(), CyclocopterUniform2D::AfterConvergence(), StructNode::AfterConvergence(), CyclocopterPolimi::AfterConvergence(), ViscousHingeJoint::AfterPredict(), ViscousDispJoint::AfterPredict(), ViscousHingeJointInv::AfterPredict(), StructNode::AfterPredict(), HydrodynamicPlainBearing::AssJac(), RodBezier::AssJac(), PlaneHingeJoint::AssJac(), BeamSliderJoint::AssJac(), AerodynamicModal::AssJac(), AerodynamicBody::AssJac(), Modal::AssJac(), AerodynamicBeam::AssJac(), AerodynamicBeam2::AssJac(), StaticMass::AssMats(), StaticVariableBody::AssMats(), StaticBody::AssMats(), ModalForce::AssRes(), Wheel2::AssRes(), StructExtForce::AssRes(), StructMappingExtForce::AssRes(), Brake::AssRes(), BeamSliderJoint::AssRes(), AeroDynModule::AssRes(), StaticMass::AssRes(), StaticVariableBody::AssRes(), AutomaticStructElem::AssRes(), Modal::AssRes(), Shell4EAS::AssRes(), Shell4EASANS::AssRes(), StaticBody::AssRes(), CyclocopterUniform2D::AssRes(), CyclocopterPolimi::AssRes(), GenericAerodynamicForce::AssVec(), AerodynamicModal::AssVec(), AerodynamicBody::AssVec(), ViscousHingeJoint::AssVec(), AerodynamicBeam::AssVec(), ViscousDispJoint::AssVec(), AerodynamicBeam2::AssVec(), ViscousHingeJointInv::AssVec(), Beam2::Beam2(), StructNode::BeforePredict(), CenterOfMass::Collect_int(), Inertia::Collect_int(), AutomaticStructDispElem::ComputeAccelerations(), AutomaticStructElem::ComputeAccelerations(), Shell4EAS::ComputeIPCurvature(), Shell4EASANS::ComputeIPCurvature(), ContactConstitutiveLaw< Vec3, Mat3x3 >::ContactConstitutiveLaw(), Modal::dGetPrivData(), DrivenElem::GetB_int(), Modal::GetB_int(), NestedElem::GetB_int(), ElemGravityOwner::GetB_int(), Modal::GetCurrFEMNodesVelocity(), RotorTrimGeneric::GetData(), DrivenElem::GetG_int(), Modal::GetG_int(), NestedElem::GetG_int(), ElemGravityOwner::GetG_int(), AutomaticStructDispElem::GetGCurr(), NoRotor::GetInducedVelocity(), CyclocopterNoInflow::GetInducedVelocity(), GlauertRotor::GetInducedVelocity(), ManglerRotor::GetInducedVelocity(), MBDynParser::GetMatR2vec(), AutomaticStructDispElem::GetS(), DrivenElem::GetS(), NestedElem::GetS(), DrivenElem::GetS_int(), ElemGravityOwner::GetS_int(), NestedElem::GetS_int(), MBDynParser::GetVec3(), Gust::GetVelocity(), StructDispNode::GetW(), StructDispNode::GetWP(), HBeam::HBeam(), Beam::Init(), ViscoElasticBeam::Init(), RodBezier::InitialAssJac(), TotalJoint::InitialAssJac(), Modal::InitialAssJac(), TotalReaction::InitialAssJac(), TotalPinJoint::InitialAssJac(), AerodynamicModal::InitialAssRes(), TotalJoint::InitialAssRes(), Modal::InitialAssRes(), TotalReaction::InitialAssRes(), TotalPinJoint::InitialAssRes(), ViscousHingeJoint::InitialAssRes(), Rotor::InitParam(), LoadIncForce::LoadIncForce(), mb_zero< Vec3 >(), ModuleNonsmoothNode::mbs_get_force(), ModuleNonsmoothNode::mbs_get_force_frictional(), ModalMappingExt::ModalMappingExt(), ModuleIMUConstraint::ModuleIMUConstraint(), ModuleNonsmoothNode::ModuleNonsmoothNode(), GimbalRotationJoint::Output(), LinearAccelerationJoint::Output(), DriveDisplacementJoint::Output(), DriveHingeJoint::Output(), LinearVelocityJoint::Output(), InLineJoint::Output(), DistanceJoint::Output(), PrismaticJoint::Output(), SphericalHingeJoint::Output(), DeformableAxialJoint::Output(), InPlaneJoint::Output(), DeformableDispJoint::Output(), RodBezier::Output(), DeformableHingeJoint::Output(), Rod::Output(), Brake::Output(), AutomaticStructDispElem::Output(), AngularAccelerationJoint::Output(), AirProperties::Output(), AngularVelocityJoint::Output(), InLineWithOffsetJoint::Output(), ContactJoint::Output(), DistanceJointWithOffset::Output(), UniversalRotationJoint::Output(), PinJoint::Output(), TotalEquation::Output(), InPlaneWithOffsetJoint::Output(), DriveDisplacementPinJoint::Output(), StructDispNode::Output(), PlaneRotationJoint::Output(), DeformableHingeJoint::OutputInv(), ScalarFuncGR::Read(), NodeDistDCR::Read(), PowerLawGR::Read(), LogarithmicGR::Read(), ReadBeam(), ReadBody(), DataManager::ReadControl(), ReadElectric(), ReadForce(), ReadGenericAerodynamicForce(), ReadGravity(), ReadJoint(), ReadModal(), DataManager::ReadOneElem(), ReadStructExtForce(), ReadStructMappingExtForce(), ReadStructNode(), ReadStructuralForce(), ModalMappingExt::Recv(), StructMappingExtForce::RecvFromFileDes(), StructMembraneMappingExtForce::RecvFromFileDes(), MBDynParser::Reference_int(), ResForces::Reset(), ModalMappingExt::Send(), ModalExt::Send(), StructNode::SetInitialValue(), StructNode::SetValue(), StructExtForce::StructExtForce(), StructMappingExtForce::StructMappingExtForce(), AircraftInstruments::Update(), ContContact3DCL::Update(), and ViscoElasticBeam2::ViscoElasticBeam2().