kernel/Operations.h File Reference

#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include "Point.h"
#include "Connection.h"

Go to the source code of this file.

Defines

#define verbose   1

Functions

void toCPT3 (BPType &val, PType &point)
void toBoost3 (PType &val, BPType &point)
void toBoost3 (const PType &val, BPType &point)
int sgn (DataType a)
int sgn2 (DataType a)
DataType norm_2 (PType val)
BPType normalize (BPType &a)
PType normalize (PType &a)
int octant (BPType a)
int octant (PType a)
int octant (BPType a, BPType b)
int octant (PType a, PType b)
int octant2 (BPType a, BPType b)
int octant2 (PType a, PType b)
BPType sin3D (BPType &a)
BPType multElem (BPType &a, BPType &b)
DataType innerProd3D (BPType &a, BPType &b)
DataType innerProd3D (PType &a, PType &b)
DataType innerProd3D (PType a, PType b)
DataType cross (BPType &a, BPType &b)
BPType cross3D (BPType &a, BPType &b)
PType cross3D (PType &a, PType &b)
BPType cross3D (BPType &a, BPType &b, int &par)
PType cross3D (PType &a, PType &b, int &par)
DataType angle3D_axis (BPType a, BPType b, BPType n)
DataType angle3D_axis (PType a, PType b, PType n)
DataType angle3D2_axis (BPType a, BPType b, BPType n)
DataType angle3D2_axis (PType a, PType b, PType n)
DataType angle3D3_axis (BPType a, BPType b, BPType n, int &flip)
DataType angle3D3_axis (PType a, PType b, PType n, int &f)
BPType rotate3D (BPType &u, BPType &theta)
bool InvertMatrix (const MType &input, MType &inverse)
DataType determinant3 (const MType &A)
BPType pointFinM (BPType p, MType A)
MType FinM (MType A)
MType tranMat3D (BPType move)
MType tranMat3D (PType move)
MType scalMat3D (BPType scale)
MType scalMat3D (PType scale)
MType rotMat3D (BPType theta)
MType rotMat3D (PType theta)
MType axisAngleMat3D (BPType axis, DataType angle)
MType axisAngleMat3D (PType axis, DataType angle)
MType axisAngleMat3DH (BPType axis, DataType angle)
MType axisAngleMat3DH (PType axis, DataType angle)
BPType matTaitBryant (MType A)
BPType angle3D2 (BPType &u, BPType &v)
void dist3D_Line_to_Line (BPType L0P0, BPType L0P1, BPType L1P0, BPType L1P1, BPType &L0I, BPType &L1I, BPType &CN, DataType &dist, int &lcase)
void dist3D_Line_to_Line (PType L0P0, PType L0P1, PType L1P0, PType L1P1, PType &L0I, PType &L1I, PType &CN, DataType &dist, int &lcase)
BPType project (BPType &u, BPType &v)
PType project (PType &u, PType &v)
DataType projectScalar (BPType &u, BPType &v)
DataType projectScalar (PType &u, PType &v)
DataType distance_point_to_line3D (BPType a, BPType b, BPType p)
DataType distance_point_to_line3D (PType a, PType b, PType p)
MType DenavitHartenbergMat (DataType radius, DataType alpha, DataType depth, DataType theta)
MType DenavitHartenbergMat_spericalWrist (DataType t4, DataType t5, DataType t6, DataType d6)
 Sperical Wrist currently works in a Euler Z-X-Z convention.
void prodMVP (MType Tnet, std::vector< BPType > &in, std::vector< BPType > &out)
void prodMVP (MType Tnet, std::vector< PType > &in, std::vector< PType > &out)
BPType prodMP (MType Tnet, BPType in)
PType prodMP (MType Tnet, PType in)
MType SkewSymmetric (BPType alpha)
void calcVelMVP (MType Tnet, std::vector< BPType > &origins, std::vector< BPType > &axis, std::vector< BPType > &angVel, std::vector< BPType > &linVel, std::vector< BPType > &in, std::vector< BPType > &out)
void numVelMVP (MType Tnet0, MType Tnet1, std::vector< BPType > &in, std::vector< BPType > &out, DataType dt)
void numVelMVP (MType Tnet0, MType Tnet1, std::vector< PType > &in, std::vector< PType > &out, DataType dt)
void printID (std::vector< int > tag)
std::string stringID (std::vector< int > tag)
void trimID (std::vector< int > &tag)
int checkTag (std::vector< int > tag)
void assignVertices3D (const std::vector< BPType > &in, ads::FixedArray< 3, DataType > *out)
void assignConnections3D (std::vector< Facet< DataType > > &in, ads::FixedArray< 3, int > *out)
PType intersection_line2facet (PType I_a, PType I_b, int &nump, int &numc, ads::FixedArray< 3, DataType > *verts, ads::FixedArray< 3, int > *conns, int &flag)
void reflect (std::vector< PType > &in, std::vector< PType > &out, PType n)
MType reflMat3D (BPType n)
MType reflMat3D (PType n)
bool fexists (const char *filename)

Define Documentation

#define verbose   1

Function Documentation

BPType angle3D2 ( BPType &  u,
BPType &  v 
)

Calculate the Tait-Bryant (Euler Z-Y-X) angles between two vectors by

  • determining if the vectors have a distinct intersect, are parallel, or are anti-parallel
    ntmp = cross3D() (u,v,par)
  • for intersecting vectors
    • find angle from u to v about ntmp
    • form a rotation matrix from the angle and ntmp
    • form Tait-Bryant angles from the rotation matrix
  • for parallel vectors
    • return (0,0,0)
  • for anti-parallel vectors
    • if u and v are not equivalent to (1,0,0) ntmp = (1,0,0)
      else ntmp normalize(rand,rand,rand)
    • find angle from u to v about ntmp
    • form a rotation matrix from the angle and ntmp
    • form Tait-Bryant angles from the rotation matrix

intersect

parallel

anti-parallel

References angle3D_axis(), axisAngleMat3DH(), cross3D(), matTaitBryant(), norm_2(), normalize(), and TOL.

DataType angle3D2_axis ( PType  a,
PType  b,
PType  n 
)

References angle3D2_axis(), and toBoost3().

DataType angle3D2_axis ( BPType  a,
BPType  b,
BPType  n 
)

Find the angle between vectors a and b about the axis n.

  • Project a and b onto a plane perpendicular to the axis as c and d.
  • Find the signed angle between c and d.

References cross3D(), d2r, norm_2(), normalize(), octant(), and TOL.

Referenced by angle3D2_axis(), and DH_Link< DataType, dim >::calcDenavitHartenbergParam().

DataType angle3D3_axis ( PType  a,
PType  b,
PType  n,
int &  f 
)

References angle3D3_axis(), and toBoost3().

DataType angle3D3_axis ( BPType  a,
BPType  b,
BPType  n,
int &  flip 
)

References cross3D(), d2r, norm_2(), normalize(), and TOL.

Referenced by angle3D3_axis(), and DH_Link< DataType, dim >::calcDenavitHartenbergParam().

DataType angle3D_axis ( PType  a,
PType  b,
PType  n 
)

References angle3D_axis(), and toBoost3().

DataType angle3D_axis ( BPType  a,
BPType  b,
BPType  n 
)

Find the angle between vectors a and b about the axis n.

  • Project a and b onto a plane perpendicular to the axis as c and d.
  • Find the angle between c and d.

References d2r, norm_2(), normalize(), and TOL.

Referenced by angle3D2(), and angle3D_axis().

void assignConnections3D ( std::vector< Facet< DataType > > &  in,
ads::FixedArray< 3, int > *  out 
)
void assignVertices3D ( const std::vector< BPType > &  in,
ads::FixedArray< 3, DataType > *  out 
)
MType axisAngleMat3D ( PType  axis,
DataType  angle 
)

References axisAngleMat3D(), and toBoost3().

MType axisAngleMat3D ( BPType  axis,
DataType  angle 
)

Form a 3x3 rotation matrix from axis - angle representation.

Referenced by axisAngleMat3D().

MType axisAngleMat3DH ( PType  axis,
DataType  angle 
)

References axisAngleMat3DH(), and toBoost3().

MType axisAngleMat3DH ( BPType  axis,
DataType  angle 
)

Form a homogeneous rotation matrix from axis - angle representation.

References d2r.

Referenced by angle3D2(), and axisAngleMat3DH().

void calcVelMVP ( MType  Tnet,
std::vector< BPType > &  origins,
std::vector< BPType > &  axis,
std::vector< BPType > &  angVel,
std::vector< BPType > &  linVel,
std::vector< BPType > &  in,
std::vector< BPType > &  out 
)

References SkewSymmetric().

int checkTag ( std::vector< int >  tag  ) 

Referenced by DH_Link< DataType, dim >::print().

DataType cross ( BPType &  a,
BPType &  b 
)

Calculate the cross product of two 2D vectors; return the resulting scalar.

References norm_2().

PType cross3D ( PType &  a,
PType &  b,
int &  par 
)

References norm_2(), octant(), toBoost3(), and TOL.

BPType cross3D ( BPType &  a,
BPType &  b,
int &  par 
)

Calculate the crossproduct of two 3D vectors; return the resulting vector and set a flag for intersecting/parallel/antiparallel.

References norm_2(), octant(), and TOL.

PType cross3D ( PType &  a,
PType &  b 
)
BPType cross3D ( BPType &  a,
BPType &  b 
)

Calculate the cross product of two 3D vectors; return the resulting vector.

Referenced by angle3D2(), angle3D2_axis(), angle3D3_axis(), DH_Link< DataType, dim >::calcDenavitHartenbergParam(), Joint< DataType, dim >::Joint(), main(), Surface< DataType >::measure(), Surface< DataType >::nearestFacet(), and Surface< DataType >::pressureForce().

MType DenavitHartenbergMat ( DataType  radius,
DataType  alpha,
DataType  depth,
DataType  theta 
)

Retrun the Denavit-Hartenberg matrix given:

Parameters:
link length (radius)
link twist (alpha)
link offset (depth)
joint angle (theta)

References d2r.

Referenced by DH_Link< DataType, dim >::calcDenavitHartenbergMat().

MType DenavitHartenbergMat_spericalWrist ( DataType  t4,
DataType  t5,
DataType  t6,
DataType  d6 
)

Sperical Wrist currently works in a Euler Z-X-Z convention.

References d2r.

Referenced by DH_Link< DataType, dim >::calcDenavitHartenbergMat().

DataType determinant3 ( const MType &  A  ) 

Calculate the determinant of a 3x3 matrix; return the result.

Referenced by intersection_line2facet().

void dist3D_Line_to_Line ( PType  L0P0,
PType  L0P1,
PType  L1P0,
PType  L1P1,
PType &  L0I,
PType &  L1I,
PType &  CN,
DataType &  dist,
int &  lcase 
)

References dist3D_Line_to_Line(), and toBoost3().

void dist3D_Line_to_Line ( BPType  L0P0,
BPType  L0P1,
BPType  L1P0,
BPType  L1P1,
BPType &  L0I,
BPType &  L1I,
BPType &  CN,
DataType &  dist,
int &  lcase 
)

dist3D_Line_to_Line(): get the 3D minimum distance between 2 lines
Input: two 3D lines L0 := L0P1 - L0P0 and L1 := L1P1 - L1P0
Return: the shortest distance between L0 and L1, intersection points, and common normal

  • lcase:
    • 0) lines intersect ( common normal is the zero vector, L0i $ \equiv $ L1i, dist = 0 )
    • 1) lines are parallel ( common normal originates from L0p0, L0P0 $ \equiv $ L0i, dist > 0 )
    • 2) lines are coincident ( common normal is the zero vector, L0P0 $ \equiv $ L0i $ \equiv $ L1i, dist = 0 )
    • 3) lines are skew ( norm_2(common normal) > 0, dist > 0 )

http://geomalgorithms.com/a07-_distance.html
http://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf

compute the line parameters of the two closest points

get the difference of the two closest points

determine lcase

coincident lines

parallel lines

instersecting lines

skew lines

References norm_2(), normalize(), and TOL.

Referenced by DH_Link< DataType, dim >::calcDenavitHartenbergParam(), and dist3D_Line_to_Line().

DataType distance_point_to_line3D ( PType  a,
PType  b,
PType  p 
)

References distance_point_to_line3D(), and toBoost3().

DataType distance_point_to_line3D ( BPType  a,
BPType  b,
BPType  p 
)

References norm_2(), normalize(), and projectScalar().

Referenced by distance_point_to_line3D(), and intersection_line2facet().

bool fexists ( const char *  filename  ) 

Referenced by MotionSolver< DataType, dim >::logLoads().

MType FinM ( MType  A  ) 

Return the transformation matrix to express stationary frame F in the coordinates of a moving frame M.
http://en.m.wikipedia.org/wiki/Kinematics

DataType innerProd3D ( PType  a,
PType  b 
)
DataType innerProd3D ( PType &  a,
PType &  b 
)
DataType innerProd3D ( BPType &  a,
BPType &  b 
)

Calculate the inner product of two 3D vectors; return the resulting scalar.

Referenced by Surface< DataType >::measure().

PType intersection_line2facet ( PType  I_a,
PType  I_b,
int &  nump,
int &  numc,
ads::FixedArray< 3, DataType > *  verts,
ads::FixedArray< 3, int > *  conns,
int &  flag 
)

References determinant3(), distance_point_to_line3D(), InvertMatrix(), norm_2(), toBoost3(), toCPT3(), and TOL.

Referenced by Assembly< DataType, dim >::surfacePoint().

bool InvertMatrix ( const MType &  input,
MType &  inverse 
)

Invert a matrix by LU decomposition; return the result.

Referenced by intersection_line2facet().

BPType matTaitBryant ( MType  A  ) 

Tait-Bryant (Euler Z-Y-X) angles from a 3x3 or homogeneous rotation matrix.

References d2r, and TOL.

Referenced by angle3D2().

BPType multElem ( BPType &  a,
BPType &  b 
)

Multiply two vectors element wise of two 3D vectors; return the resulting vector.

DataType norm_2 ( PType  val  )  [inline]

References toBoost3().

Referenced by Spline< DataType, dim >::AddSplineNode(), angle3D2(), angle3D2_axis(), angle3D3_axis(), angle3D_axis(), DH_Link< DataType, dim >::calcDenavitHartenbergParam(), cross(), cross3D(), dist3D_Line_to_Line(), distance_point_to_line3D(), intersection_line2facet(), Loft< DataType >::makeUnique(), Curve< DataType, dim >::makeUnique(), Surface< DataType >::measure(), Curve< DataType, dim >::measure(), Surface< DataType >::nearestFacet(), normalize(), projectScalar(), Surface< DataType >::read(), SplineCurve< DataType, dim >::read(), Loft< DataType >::read(), Curve< DataType, dim >::read(), and Surface< DataType >::validFacet().

PType normalize ( PType &  a  ) 

References normalize(), toBoost3(), and toCPT3().

BPType normalize ( BPType &  a  ) 

Normalized a vector; return the result.

References norm_2(), and TOL.

Referenced by angle3D2(), angle3D2_axis(), angle3D3_axis(), angle3D_axis(), DH_Link< DataType, dim >::calcDenavitHartenbergParam(), dist3D_Line_to_Line(), distance_point_to_line3D(), Joint< DataType, dim >::Joint(), Spline< DataType, dim >::linear(), normalize(), Assembly< DataType, dim >::surfacePoint(), and Surface< DataType >::updateAxis().

void numVelMVP ( MType  Tnet0,
MType  Tnet1,
std::vector< PType > &  in,
std::vector< PType > &  out,
DataType  dt 
)
void numVelMVP ( MType  Tnet0,
MType  Tnet1,
std::vector< BPType > &  in,
std::vector< BPType > &  out,
DataType  dt 
)

Referenced by Surface< DataType >::updatePart(), and Loft< DataType >::updatePart().

int octant ( PType  a,
PType  b 
) [inline]

References octant(), and toBoost3().

int octant ( BPType  a,
BPType  b 
) [inline]

Check if two vectors are in the same octant; return 1 if they are 0 otherwise.

References sgn().

int octant ( PType  a  )  [inline]

References octant(), and toBoost3().

int octant ( BPType  a  )  [inline]

Return which ocatnt a vectors is in.

References sgn().

Referenced by angle3D2_axis(), cross3D(), Surface< DataType >::nearestFacet(), and octant().

int octant2 ( PType  a,
PType  b 
) [inline]

References octant2(), and toBoost3().

int octant2 ( BPType  a,
BPType  b 
) [inline]

References sgn().

Referenced by DH_Link< DataType, dim >::calcDenavitHartenbergParam(), and octant2().

BPType pointFinM ( BPType  p,
MType  A 
)

Return the position of a point in a stationary frame F in the coordinates of a moving frame M.
http://en.m.wikipedia.org/wiki/Kinematics

void printID ( std::vector< int >  tag  ) 

Referenced by Assembly< DataType, dim >::AddPart(), Assembly< DataType, dim >::AddSubAssembly(), main(), and trimID().

PType prodMP ( MType  Tnet,
PType  in 
)

References toCPT3().

BPType prodMP ( MType  Tnet,
BPType  in 
)

Referenced by Loft< DataType >::GenLoft(), and Surface< DataType >::updateAxis().

void prodMVP ( MType  Tnet,
std::vector< PType > &  in,
std::vector< PType > &  out 
)
void prodMVP ( MType  Tnet,
std::vector< BPType > &  in,
std::vector< BPType > &  out 
)

Referenced by Loft< DataType >::GenLoft(), Loft< DataType >::read(), Surface< DataType >::updatePart(), and Loft< DataType >::updatePart().

PType project ( PType &  u,
PType &  v 
)

References project(), toBoost3(), and toCPT3().

BPType project ( BPType &  u,
BPType &  v 
)

Retrun the projection of one 3D vector, u, onto another, v.

Referenced by DH_Link< DataType, dim >::calcDenavitHartenbergParam(), and project().

DataType projectScalar ( PType &  u,
PType &  v 
)

References projectScalar(), and toBoost3().

DataType projectScalar ( BPType &  u,
BPType &  v 
)

References norm_2().

Referenced by distance_point_to_line3D(), Surface< DataType >::logLoad(), and projectScalar().

void reflect ( std::vector< PType > &  in,
std::vector< PType > &  out,
PType  n 
)

reflection about a plane through the origin with normal n = (a,b,c)

MType reflMat3D ( PType  n  ) 

References reflMat3D(), and toBoost3().

MType reflMat3D ( BPType  n  ) 

Form a homogeneous reflection matrix from vector.

Referenced by Loft< DataType >::read(), and reflMat3D().

BPType rotate3D ( BPType &  u,
BPType &  theta 
)

Rotate a 3D vector by a 3D extrinsic angles (degrees); return the resulting vector.

References d2r.

MType rotMat3D ( PType  theta  ) 

References rotMat3D(), and toBoost3().

MType rotMat3D ( BPType  theta  ) 

Form a homogeneous rotation matrix from Tait-Braynt (Euler Z-X-Y) angles.

References d2r.

Referenced by Loft< DataType >::GenLoft(), and rotMat3D().

MType scalMat3D ( PType  scale  ) 

References scalMat3D(), and toBoost3().

MType scalMat3D ( BPType  scale  ) 

Form a homogeneous scaling matrix from vector.

Referenced by Loft< DataType >::GenLoft(), and scalMat3D().

int sgn ( DataType  a  )  [inline]

References TOL.

Referenced by octant(), and octant2().

int sgn2 ( DataType  a  )  [inline]
BPType sin3D ( BPType &  a  ) 

Calculate the std::sin() of each vector component; return the result.

MType SkewSymmetric ( BPType  alpha  ) 

Referenced by calcVelMVP().

std::string stringID ( std::vector< int >  tag  ) 

Referenced by Assembly< DataType, dim >::AddPart(), Assembly< DataType, dim >::AddSubAssembly(), Assembly< DataType, dim >::Checkpointing(), DH_Link< DataType, dim >::DH_Link(), main(), Assembly< DataType, dim >::print(), Assembly< DataType, dim >::printSummary(), and Assembly< DataType, dim >::Restart().

void toBoost3 ( const PType &  val,
BPType &  point 
) [inline]
void toBoost3 ( PType &  val,
BPType &  point 
) [inline]

Referenced by angle3D2_axis(), angle3D3_axis(), angle3D_axis(), axisAngleMat3D(), axisAngleMat3DH(), cross3D(), dist3D_Line_to_Line(), distance_point_to_line3D(), Spline< DataType, dim >::Eq(), intersection_line2facet(), Spline< DataType, dim >::linear(), norm_2(), normalize(), octant(), octant2(), project(), projectScalar(), reflMat3D(), rotMat3D(), scalMat3D(), and tranMat3D().

void toCPT3 ( BPType &  val,
PType &  point 
) [inline]

Referenced by Spline< DataType, dim >::Eq(), intersection_line2facet(), Spline< DataType, dim >::linear(), normalize(), prodMP(), and project().

MType tranMat3D ( PType  move  ) 

References toBoost3(), and tranMat3D().

MType tranMat3D ( BPType  move  ) 

Form a homogeneous translation matrix from vector.

Referenced by Loft< DataType >::GenLoft(), and tranMat3D().

void trimID ( std::vector< int > &  tag  ) 

References printID().

Generated on Thu Jun 30 02:18:58 2016 for Motion - rigid body dynamics solver by  doxygen 1.6.3