double el(double z,double r0, double el0, double w0, double energy)
{
// GSM beam coherence width
double lambda = sqrt((1.5 * pow(10,-18))/(energy));
// again, really small value; should this be global? value containing the wavelength; wavelength of what?
double w;
w = (el0) * (fabs((z)/(zp(z,r0))) * ((sqrt((1 + (pow(((lambda * zp(z,r0))/(el0 * w0)),2)))))));
return(w);
}
double v(double z,double r0, double el0, double w0, double energy)
{
// compute GSM radius of wavefront curvature
double lambda = sqrt((1.5 * pow(10,-18))/(energy));
// value containing wavelength; this is approx. wavelength of xrays.
double v;
v=(z)/(1-zp(z,r0)/(z * (1 + pow(((lambda * zp(z,r0)/(el0 * w0))),2))));
return(v);
}
double w(double z,double r0, double el0, double w0, double energy)
{
// Compute GSM beam width (GSM = Gaussian-Schell Model of gratings)
double lambda = sqrt((1.5 * pow(10,-18))/(energy));
// variable containing value of the wavelength; wavelength of what?
double w;
w = (el0) * (fabs((z)/(zp(z,r0))) * ((sqrt((1 + (pow(((lambda * zp(z,r0))/(el0 * w0)),2)))))));
// what does this correspond to?
return(w);
}
void NodePalette::ToggleAbsMode()
{
if (doc==0)
return;
FPoint zp(0.0, 0.0);
disconnect(XSpin, SIGNAL(valueChanged(double)), this, SLOT(MovePoint()));
disconnect(YSpin, SIGNAL(valueChanged(double)), this, SLOT(MovePoint()));
FPointArray Clip;
FPoint np(0.0, 0.0);
if (EditCont->isChecked())
Clip = doc->m_Selection->itemAt(0)->ContourLine;
else
Clip = doc->m_Selection->itemAt(0)->PoLine;
if (doc->nodeEdit.SelNode.count() != 0)
np = Clip.point(doc->nodeEdit.SelNode.at(0));
if (AbsMode->isChecked())
{
XSpin->setMinimum(-16777215);
YSpin->setMinimum(-16777215);
if (absToCanvas->isChecked())
zp = FPoint(doc->m_Selection->itemAt(0)->xPos(), doc->m_Selection->itemAt(0)->yPos());
else
zp = FPoint(doc->m_Selection->itemAt(0)->xPos() - doc->currentPage()->xOffset(), doc->m_Selection->itemAt(0)->yPos() - doc->currentPage()->yOffset());
}
else
{
XSpin->setMinimum(0);
YSpin->setMinimum(0);
}
XSpin->setValue((np.x() + zp.x())*doc->unitRatio());
YSpin->setValue((np.y() + zp.y())*doc->unitRatio());
connect(XSpin, SIGNAL(valueChanged(double)), this, SLOT(MovePoint()));
connect(YSpin, SIGNAL(valueChanged(double)), this, SLOT(MovePoint()));
}
osgToy::OctoStrip::OctoStrip()
{
osg::Vec3Array* vAry = dynamic_cast<osg::Vec3Array*>( getVertexArray() );
osg::Vec3Array* nAry = dynamic_cast<osg::Vec3Array*>( getNormalArray() );
setNormalBinding( osg::Geometry::BIND_PER_VERTEX );
osg::Vec4Array* cAry = dynamic_cast<osg::Vec4Array*>( getColorArray() );
setColorBinding( osg::Geometry::BIND_PER_VERTEX );
osg::Vec3 xp( 1, 0, 0); osg::Vec4 red(1,0,0,1);
osg::Vec3 xn(-1, 0, 0); osg::Vec4 cyan(0,1,1,1);
osg::Vec3 yp( 0, 1, 0); osg::Vec4 green(0,1,0,1);
osg::Vec3 yn( 0,-1, 0); osg::Vec4 magenta(1,0,1,1);
osg::Vec3 zp( 0, 0, 1); osg::Vec4 blue(0,0,1,1);
osg::Vec3 zn( 0, 0,-1); osg::Vec4 yellow(1,1,0,1);
vAry->push_back(zp); nAry->push_back(zp); cAry->push_back(blue);
vAry->push_back(yp); nAry->push_back(yp); cAry->push_back(green);
vAry->push_back(xn); nAry->push_back(xn); cAry->push_back(cyan);
vAry->push_back(zn); nAry->push_back(zn); cAry->push_back(yellow);
vAry->push_back(yn); nAry->push_back(yn); cAry->push_back(magenta);
vAry->push_back(xp); nAry->push_back(xp); cAry->push_back(red);
vAry->push_back(zp); nAry->push_back(zp); cAry->push_back(blue);
vAry->push_back(yp); nAry->push_back(yp); cAry->push_back(green);
addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_STRIP, 0, vAry->size() ) );
}
void vkeGameRendererDynamic::initCamera(){
nv_math::vec4f zp(0.0, 0.0, 0.0, 1.0);
m_camera = new VkeCamera(1, -20.0, -1.0, -8.0);
m_camera->lookAt(zp);
m_camera->update();
m_light = new VkeCamera(2, -6, -6, -10);
m_light->lookAt(zp);
m_light->update();
}
void NodePalette::SetXY(double x, double y)
{
if (doc==0)
return;
FPoint zp(0.0, 0.0);
disconnect(XSpin, SIGNAL(valueChanged(double)), this, SLOT(MovePoint()));
disconnect(YSpin, SIGNAL(valueChanged(double)), this, SLOT(MovePoint()));
if (AbsMode->isChecked())
{
if (absToCanvas->isChecked())
zp = FPoint(doc->m_Selection->itemAt(0)->xPos(), doc->m_Selection->itemAt(0)->yPos());
else
zp = FPoint(doc->m_Selection->itemAt(0)->xPos() - doc->currentPage()->xOffset(), doc->m_Selection->itemAt(0)->yPos() - doc->currentPage()->yOffset());
}
XSpin->setValue((x + zp.x())*doc->unitRatio());
YSpin->setValue((y + zp.y())*doc->unitRatio());
connect(XSpin, SIGNAL(valueChanged(double)), this, SLOT(MovePoint()));
connect(YSpin, SIGNAL(valueChanged(double)), this, SLOT(MovePoint()));
}
void
GolemMaterialBase::computeRotationMatrix()
{
RealVectorValue xp, yp, zp;
xp = _current_elem->point(1) - _current_elem->point(0);
switch (_material_type)
{
case 1:
for (unsigned int i = 0; i < 3; ++i)
yp(i) = 0.0;
if (std::fabs(xp(0)) > 0.0 && std::fabs(xp(1)) + std::fabs(xp(2)) < DBL_MIN)
yp(2) = 1.0;
else if (std::fabs(xp(1)) > 0.0 && std::fabs(xp(0)) + std::fabs(xp(2)) < DBL_MIN)
yp(0) = 1.0;
else if (std::fabs(xp(2)) > 0.0 && std::fabs(xp(0)) + std::fabs(xp(1)) < DBL_MIN)
yp(1) = 1.0;
else
{
for (unsigned int i = 0; i < 3; ++i)
if (std::fabs(xp(i)) > 0.0)
{
yp(i) = -xp(i);
break;
}
}
break;
case 2:
yp = _current_elem->point(2) - _current_elem->point(1);
break;
case 3:
break;
}
zp = xp.cross(yp);
yp = zp.cross(xp);
for (unsigned int i = 0; i < 3; ++i)
{
(_rotation_matrix)(i, 0) = xp(i) / xp.norm();
(_rotation_matrix)(i, 1) = yp(i) / yp.norm();
(_rotation_matrix)(i, 2) = zp(i) / zp.norm();
}
}
osgToy::RhombicDodecahedron::RhombicDodecahedron()
{
setOverallColor( osg::Vec4(0,1,0,1) );
osg::Vec3 a0( 1, 1, 1 );
osg::Vec3 a1( 1, 1, -1 );
osg::Vec3 a2( 1, -1, 1 );
osg::Vec3 a3( 1, -1, -1 );
osg::Vec3 a4( -1, 1, 1 );
osg::Vec3 a5( -1, 1, -1 );
osg::Vec3 a6( -1, -1, 1 );
osg::Vec3 a7( -1, -1, -1 );
osg::Vec3 xp( 2, 0, 0 );
osg::Vec3 xn( -2, 0, 0 );
osg::Vec3 yp( 0, 2, 0 );
osg::Vec3 yn( 0, -2, 0 );
osg::Vec3 zp( 0, 0, 2 );
osg::Vec3 zn( 0, 0, -2 );
addTristrip( yp, a0, a1, xp );
addTristrip( xp, a2, a3, yn );
addTristrip( yn, a6, a7, xn );
addTristrip( xn, a4, a5, yp );
addTristrip( zp, a0, a4, yp );
addTristrip( yp, a1, a5, zn );
addTristrip( zn, a3, a7, yn );
addTristrip( yn, a2, a6, zp );
addTristrip( xp, a0, a2, zp );
addTristrip( zp, a4, a6, xn );
addTristrip( xn, a5, a7, zn );
addTristrip( zn, a1, a3, xp );
osgToy::FacetingVisitor::facet( *this );
}
/*
* Numerical Recipes quotes a formula due to Rybicki for evaluating
* Dawson's Integral:
*
* exp(-x^2) integral exp(t^2).dt = 1/sqrt(pi) lim sum exp(-(z-n.h)^2) / n
* 0 to x h->0 n odd
*
* This can be adapted to erf(z).
*/
std::complex<double> cerf_rybicki(const std::complex<double>& z)
{
double h = 0.2; // numerical experiment suggests this is small enough
// choose an even n0, and then shift z->z-n0.h and n->n-h.
// n0 is chosen so that real((z-n0.h)^2) is as small as possible.
int n0 = 2 * static_cast<int>(z.imag() / (2 * h) + 0.5);
std::complex<double> z0(0.0, n0 * h);
std::complex<double> zp(z - z0);
std::complex<double> sum(0.0, 0.0);
// limits of sum chosen so that the end sums of the sum are
// fairly small. In this case exp(-(35.h)^2)=5e-22
for (int np = -35; np <= 35; np += 2) {
std::complex<double> t(zp.real(), zp.imag() - np * h);
std::complex<double> b(exp(t * t) / static_cast<double>(np + n0));
sum += b;
}
sum *= 2.0 * exp(-z * z) / pi;
return std::complex<double>(-sum.imag(), sum.real());
}
void SolveSLE(const Matrix &A, Matrix &Z, const Matrix &b, float initialZ)
{
int aCols = A.GetColsCount();
int aRows = A.GetRowsCount();
int zCols = Z.GetColsCount();
int zRows = Z.GetRowsCount();
int bCols = b.GetColsCount();
int bRows = b.GetRowsCount();
if (aCols != aRows || zCols != 1 || bCols != 1 || zRows != bRows || zRows != aCols)
throw("SLE solver: This is not a SLE!\n");
printf(" Solving SLE ... \n");
clock_t time = clock();
int m = aCols;
float normb = b.Norm();
Matrix rk(m, 1);
Matrix pk(m, 1);
Matrix Ark(m, 1);
Matrix Apk(m, 1);
Matrix zp(m, 1);
Matrix zpp(m, 1);
Matrix tmpV(m, 1);
zpp.Fill(initialZ);
pk.Fill(0.0f);
rk.Fill(0.0f);
Ark.Fill(0.0f);
Apk.Fill(0.0f);
zp.Fill(0.0f);
tmpV.Fill(0.0f);
rk = A * zpp - b;
float normr = rk.Norm();
Ark = A * rk;
float d = Ark.Dot(rk);
zp = zpp - rk * (normr * normr / d);
int flag = 1;
int iterations = 1;
while (flag == 1)
{
rk = A * zp - b;
normr = rk.Norm();
pk = zp - zpp;
Ark = A * rk;
Apk = A * pk;
float dot1 = Ark.Dot(pk);
float dot2 = rk.Dot(pk);
float dot3 = Ark.Dot(rk);
float dot4 = Apk.Dot(pk);
d = dot3 * dot4 - dot1 * dot1;
float gamma = ((normr * normr) * dot4 - dot2 * dot1) / d;
float beta = ((normr * normr) * dot1 - dot2 * dot3) / d;
zpp = zp;
zp -= rk * gamma - pk * beta;
tmpV = A * zp - b;
double norm = tmpV.Norm();
double error = norm / normb;
printf(" Iteration:%d\terror:%f\n", iterations, error);
if (error < 0.0001)
flag = 0;
iterations++;
}
printf(" SLE solved with %d iterations for %f\n", iterations, (double)(clock() - time) / (CLOCKS_PER_SEC * 60));
Z = zp;
return;
}
int Sphere::Triangulate(float3 *outPos, float3 *outNormal, float2 *outUV, int numVertices, bool ccwIsFrontFacing) const
{
assume(outPos);
assume(numVertices >= 24 && "At minimum, sphere triangulation will contain at least 8 triangles, which is 24 vertices, but fewer were specified!");
assume(numVertices % 3 == 0 && "Warning:: The size of output should be divisible by 3 (each triangle takes up 3 vertices!)");
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
if (!outPos)
return 0;
#endif
assume(this->r > 0.f);
if (numVertices < 24)
return 0;
#ifdef MATH_ENABLE_STL_SUPPORT
std::vector<Triangle> temp;
#else
Array<Triangle> temp;
#endif
// Start subdividing from a diamond shape.
float3 xp(r,0,0);
float3 xn(-r,0,0);
float3 yp(0,r,0);
float3 yn(0,-r,0);
float3 zp(0,0,r);
float3 zn(0,0,-r);
if (ccwIsFrontFacing)
{
temp.push_back(Triangle(yp,xp,zp));
temp.push_back(Triangle(xp,yp,zn));
temp.push_back(Triangle(yn,zp,xp));
temp.push_back(Triangle(yn,xp,zn));
temp.push_back(Triangle(zp,xn,yp));
temp.push_back(Triangle(yp,xn,zn));
temp.push_back(Triangle(yn,xn,zp));
temp.push_back(Triangle(xn,yn,zn));
}
else
{
temp.push_back(Triangle(yp,zp,xp));
temp.push_back(Triangle(xp,zn,yp));
temp.push_back(Triangle(yn,xp,zp));
temp.push_back(Triangle(yn,zn,xp));
temp.push_back(Triangle(zp,yp,xn));
temp.push_back(Triangle(yp,zn,xn));
temp.push_back(Triangle(yn,zp,xn));
temp.push_back(Triangle(xn,zn,yn));
}
int oldEnd = 0;
while(((int)temp.size()-oldEnd+3)*3 <= numVertices)
{
Triangle cur = temp[oldEnd];
float3 a = ((cur.a + cur.b) * 0.5f).ScaledToLength(this->r);
float3 b = ((cur.a + cur.c) * 0.5f).ScaledToLength(this->r);
float3 c = ((cur.b + cur.c) * 0.5f).ScaledToLength(this->r);
temp.push_back(Triangle(cur.a, a, b));
temp.push_back(Triangle(cur.b, c, a));
temp.push_back(Triangle(cur.c, b, c));
temp.push_back(Triangle(a, c, b));
++oldEnd;
}
// Check that we really did tessellate as many new triangles as possible.
assert(((int)temp.size()-oldEnd)*3 <= numVertices && ((int)temp.size()-oldEnd)*3 + 9 > numVertices);
for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
{
outPos[3*j] = this->pos + temp[i].a;
outPos[3*j+1] = this->pos + temp[i].b;
outPos[3*j+2] = this->pos + temp[i].c;
}
if (outNormal)
for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
{
outNormal[3*j] = temp[i].a.Normalized();
outNormal[3*j+1] = temp[i].b.Normalized();
outNormal[3*j+2] = temp[i].c.Normalized();
}
if (outUV)
for(size_t i = oldEnd, j = 0; i < temp.size(); ++i, ++j)
{
outUV[3*j] = float2(atan2(temp[i].a.y, temp[i].a.x) / (2.f * 3.141592654f) + 0.5f, (temp[i].a.z + r) / (2.f * r));
outUV[3*j+1] = float2(atan2(temp[i].b.y, temp[i].b.x) / (2.f * 3.141592654f) + 0.5f, (temp[i].b.z + r) / (2.f * r));
outUV[3*j+2] = float2(atan2(temp[i].c.y, temp[i].c.x) / (2.f * 3.141592654f) + 0.5f, (temp[i].c.z + r) / (2.f * r));
}
return ((int)temp.size() - oldEnd) * 3;
}
void MG_poseReader::draw( M3dView & view, const MDagPath & path,
M3dView::DisplayStyle dispStyle,
M3dView::DisplayStatus status )
{
MPlug sizeP (thisMObject(),size);
double sizeV;
sizeP.getValue(sizeV);
MPlug poseMatrixP (thisMObject(),poseMatrix);
MObject poseMatrixData;
poseMatrixP.getValue(poseMatrixData);
MFnMatrixData matrixFn(poseMatrixData);
MMatrix poseMatrixV =matrixFn.matrix();
MPlug readerMatrixP (thisMObject(),readerMatrix);
MObject readerMatrixData;
readerMatrixP.getValue(readerMatrixData);
matrixFn.setObject(readerMatrixData);
MMatrix readerMatrixV =matrixFn.matrix();
MMatrix poseMatrixFix =poseMatrixV*readerMatrixV.inverse();
MPlug aimAxisP (thisMObject(),aimAxis);
int aimAxisV;
aimAxisP.getValue(aimAxisV);
MVector aimBall;
MPlug readerOnOffP(thisMObject(),readerOnOff);
MPlug axisOnOffP(thisMObject(),axisOnOff);
MPlug poseOnOffP(thisMObject(),poseOnOff);
double readerOnOffV;
double axisOnOffV;
double poseOnOffV;
readerOnOffP.getValue(readerOnOffV);
axisOnOffP.getValue(axisOnOffV);
poseOnOffP.getValue(poseOnOffV);
MPlug xPositiveP (thisMObject(),xPositive);
MPlug xNegativeP (thisMObject(),xNegative);
double xPositiveV;
double xNegativeV;
xPositiveP.getValue(xPositiveV);
xNegativeP.getValue(xNegativeV);
double xColor = xPositiveV;
if (xPositiveV==0)
{
xColor=xNegativeV;
}
MPlug yPositiveP (thisMObject(),yPositive);
MPlug yNegativeP (thisMObject(),yNegative);
double yPositiveV;
double yNegativeV;
yPositiveP.getValue(yPositiveV);
yNegativeP.getValue(yNegativeV);
double yColor = yPositiveV;
if (yPositiveV==0)
{
yColor=yNegativeV;
}
MPlug zPositiveP (thisMObject(),zPositive);
MPlug zNegativeP (thisMObject(),zNegative);
double zPositiveV;
double zNegativeV;
zPositiveP.getValue(zPositiveV);
zNegativeP.getValue(zNegativeV);
double zColor = zPositiveV;
if (zPositiveV==0)
{
zColor=zNegativeV;
}
if (aimAxisV==0)
{
aimBall.x=poseMatrixFix[0][0];
aimBall.y=poseMatrixFix[0][1];
aimBall.z=poseMatrixFix[0][2];
}
else if (aimAxisV==1)
{
aimBall.x=poseMatrixFix[1][0];
aimBall.y=poseMatrixFix[1][1];
aimBall.z=poseMatrixFix[1][2];
}else
{
aimBall.x=poseMatrixFix[2][0];
aimBall.y=poseMatrixFix[2][1];
aimBall.z=poseMatrixFix[2][2];
}
//*****************************************************************
// Initialize opengl and draw
//*****************************************************************
view.beginGL();
glPushAttrib( GL_ALL_ATTRIB_BITS );
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
glLineWidth(2);
if(status == M3dView::kLead)
glColor4f(0.0,1.0,0.0,0.3f);
else
glColor4f(1.0,1.0,0.0,0.3f);
MVector baseV(0,0,0);
MVector xp(1*sizeV,0,0);
MVector xm(-1*sizeV,0,0);
MVector yp(0,1*sizeV,0);
MVector ym(0,-1*sizeV,0);
MVector zp(0,0,1*sizeV);
MVector zm(0,0,-1*sizeV);
double * red;
red = new double[4];
red[0]=1;
red[1]=0;
red[2]=0;
red[3]=1;
double * green;
green = new double[4];
green[0]=0;
green[1]=1;
green[2]=0;
green[3]=1;
double * blue;
blue = new double[4];
blue[0]=0;
blue[1]=0;
blue[2]=1;
blue[3]=1;
double * yellow;
yellow = new double[4];
yellow[0]=1;
yellow[1]=1;
yellow[2]=0.2;
yellow[3]=0.3;
if (readerOnOffV==1)
{
drawSphere(sizeV,20,20,baseV,yellow);
}
if (axisOnOffV==1)
{
drawSphere(sizeV/7,15,15,xp,red);
drawSphere(sizeV/7,15,15,xm,red);
drawSphere(sizeV/7,15,15,yp,green);
drawSphere(sizeV/7,15,15,ym,green);
drawSphere(sizeV/7,15,15,zp,blue);
drawSphere(sizeV/7,15,15,zm,blue);
}
if (poseOnOffV==1)
{
double* color = blendColor(xColor,yColor,zColor,1);
drawSphere(sizeV/7,15,15,aimBall*sizeV,color);
}
glDisable(GL_BLEND);
glPopAttrib();
}
Real
MaterialTensorAux::getTensorQuantity(const SymmTensor & tensor,
const MTA_ENUM quantity,
const MooseEnum & quantity_moose_enum,
const int index,
const Point * curr_point,
const Point * p1,
const Point * p2)
{
Real value(0);
if (quantity == MTA_COMPONENT)
{
value = tensor.component(index);
}
else if ( quantity == MTA_VONMISES )
{
value = std::sqrt(0.5*(
std::pow(tensor.xx() - tensor.yy(), 2) +
std::pow(tensor.yy() - tensor.zz(), 2) +
std::pow(tensor.zz() - tensor.xx(), 2) + 6 * (
std::pow(tensor.xy(), 2) +
std::pow(tensor.yz(), 2) +
std::pow(tensor.zx(), 2))));
}
else if ( quantity == MTA_PLASTICSTRAINMAG )
{
value = std::sqrt(2.0/3.0 * tensor.doubleContraction(tensor));
}
else if ( quantity == MTA_HYDROSTATIC )
{
value = tensor.trace()/3.0;
}
else if ( quantity == MTA_HOOP )
{
// This is the location of the stress. A vector from the cylindrical axis to this point will define the x' axis.
Point p0( *curr_point );
// The vector p1 + t*(p2-p1) defines the cylindrical axis. The point along this
// axis closest to p0 is found by the following for t:
const Point p2p1( *p2 - *p1 );
const Point p2p0( *p2 - p0 );
const Point p1p0( *p1 - p0 );
const Real t( -(p1p0*p2p1)/p2p1.size_sq() );
// The nearest point on the cylindrical axis to p0 is p.
const Point p( *p1 + t * p2p1 );
Point xp( p0 - p );
xp /= xp.size();
Point yp( p2p1/p2p1.size() );
Point zp( xp.cross( yp ));
//
// The following works but does more than we need
//
// // Rotation matrix R
// ColumnMajorMatrix R(3,3);
// // Fill with direction cosines
// R(0,0) = xp(0);
// R(1,0) = xp(1);
// R(2,0) = xp(2);
// R(0,1) = yp(0);
// R(1,1) = yp(1);
// R(2,1) = yp(2);
// R(0,2) = zp(0);
// R(1,2) = zp(1);
// R(2,2) = zp(2);
// // Rotate
// ColumnMajorMatrix tensor( _tensor[_qp].columnMajorMatrix() );
// ColumnMajorMatrix tensorp( R.transpose() * ( tensor * R ));
// // Hoop stress is the zz stress
// value = tensorp(2,2);
//
// Instead, tensorp(2,2) = R^T(2,:)*tensor*R(:,2)
//
const Real zp0( zp(0) );
const Real zp1( zp(1) );
const Real zp2( zp(2) );
value = (zp0*tensor(0,0)+zp1*tensor(1,0)+zp2*tensor(2,0))*zp0 +
(zp0*tensor(0,1)+zp1*tensor(1,1)+zp2*tensor(2,1))*zp1 +
(zp0*tensor(0,2)+zp1*tensor(1,2)+zp2*tensor(2,2))*zp2;
}
else if ( quantity == MTA_RADIAL )
{
// This is the location of the stress. A vector from the cylindrical axis to this point will define the x' axis
// which is the radial direction in which we want the stress.
Point p0( *curr_point );
// The vector p1 + t*(p2-p1) defines the cylindrical axis. The point along this
// axis closest to p0 is found by the following for t:
const Point p2p1( *p2 - *p1 );
const Point p2p0( *p2 - p0 );
const Point p1p0( *p1 - p0 );
const Real t( -(p1p0*p2p1)/p2p1.size_sq() );
// The nearest point on the cylindrical axis to p0 is p.
const Point p( *p1 + t * p2p1 );
Point xp( p0 - p );
xp /= xp.size();
const Real xp0( xp(0) );
const Real xp1( xp(1) );
const Real xp2( xp(2) );
value = (xp0*tensor(0,0)+xp1*tensor(1,0)+xp2*tensor(2,0))*xp0 +
(xp0*tensor(0,1)+xp1*tensor(1,1)+xp2*tensor(2,1))*xp1 +
(xp0*tensor(0,2)+xp1*tensor(1,2)+xp2*tensor(2,2))*xp2;
}
else if ( quantity == MTA_AXIAL )
{
// The vector p2p1=(p2-p1) defines the axis, which is the direction in which we want the stress.
Point p2p1( *p2 - *p1 );
p2p1 /= p2p1.size();
const Real axis0( p2p1(0) );
const Real axis1( p2p1(1) );
const Real axis2( p2p1(2) );
value = (axis0*tensor(0,0)+axis1*tensor(1,0)+axis2*tensor(2,0))*axis0 +
(axis0*tensor(0,1)+axis1*tensor(1,1)+axis2*tensor(2,1))*axis1 +
(axis0*tensor(0,2)+axis1*tensor(1,2)+axis2*tensor(2,2))*axis2;
}
else if ( quantity == MTA_MAXPRINCIPAL )
{
value = principalValue( tensor, 0 );
}
else if ( quantity == MTA_MEDPRINCIPAL )
{
value = principalValue( tensor, 1 );
}
else if ( quantity == MTA_MINPRINCIPAL )
{
value = principalValue( tensor, 2 );
}
else if ( quantity == MTA_FIRSTINVARIANT )
{
value = tensor.trace();
}
else if ( quantity == MTA_SECONDINVARIANT )
{
value =
tensor.xx()*tensor.yy() +
tensor.yy()*tensor.zz() +
tensor.zz()*tensor.xx() -
tensor.xy()*tensor.xy() -
tensor.yz()*tensor.yz() -
tensor.zx()*tensor.zx();
}
else if ( quantity == MTA_THIRDINVARIANT )
{
value =
tensor.xx()*tensor.yy()*tensor.zz() -
tensor.xx()*tensor.yz()*tensor.yz() +
tensor.xy()*tensor.zx()*tensor.yz() -
tensor.xy()*tensor.xy()*tensor.zz() +
tensor.zx()*tensor.xy()*tensor.yz() -
tensor.zx()*tensor.zx()*tensor.yy();
}
else if ( quantity == MTA_TRIAXIALITY )
{
Real hydrostatic = tensor.trace()/3.0;
Real von_mises = std::sqrt(0.5*(
std::pow(tensor.xx() - tensor.yy(), 2) +
std::pow(tensor.yy() - tensor.zz(), 2) +
std::pow(tensor.zz() - tensor.xx(), 2) + 6 * (
std::pow(tensor.xy(), 2) +
std::pow(tensor.yz(), 2) +
std::pow(tensor.zx(), 2))));
value = std::abs(hydrostatic / von_mises);
}
else if ( quantity == MTA_VOLUMETRICSTRAIN )
{
value =
tensor.trace() +
tensor.xx()*tensor.yy() +
tensor.yy()*tensor.zz() +
tensor.zz()*tensor.xx() +
tensor.xx()*tensor.yy()*tensor.zz();
}
else
{
mooseError("Unknown quantity in MaterialTensorAux: " + quantity_moose_enum.operator std::string());
}
return value;
}
void
ckbot::chain_rate::tip_base_step(std::vector<double> s, std::vector<double> T)
{
int N = c.num_links();
std::vector<double> q(N);
std::vector<double> qd(N);
for(int i=0; i<N; ++i)
{
q[i] = s[i];
qd[i] = s[N+i];
}
Eigen::Matrix3d Iden = Eigen::Matrix3d::Identity();
Eigen::Matrix3d Zero = Eigen::Matrix3d::Zero();
/* Declare and initialized all of the loop variables */
/* TODO: Can we allocate all of this on the heap *once* for a particular
* rate object, and then just use pointers to them after? Would this be fast?
* Re-initializing these every time through here has to be slow...
*/
Eigen::MatrixXd pp(6,6);
pp = Eigen::MatrixXd::Zero(6,6);
Eigen::VectorXd zp(6);
zp << 0,0,0,0,0,0;
Eigen::VectorXd grav(6);
grav << 0,0,0,0,0,9.81;
Eigen::Matrix3d L_oc_tilde;
Eigen::Matrix3d R_cur;
Eigen::MatrixXd M_cur(6,6);
Eigen::MatrixXd M_cm(6,6);
M_cm = Eigen::MatrixXd::Zero(6,6);
Eigen::Matrix3d J_o;
Eigen::Vector3d r_i_ip(0,0,0);
Eigen::Vector3d r_i_cm(0,0,0);
Eigen::MatrixXd phi(6,6);
Eigen::MatrixXd phi_cm(6,6);
Eigen::MatrixXd p_cur(6,6);
Eigen::VectorXd H_b_frame_star(6);
Eigen::VectorXd H_w_frame_star(6);
Eigen::RowVectorXd H(6);
double D = 0.0;
Eigen::VectorXd G(6);
Eigen::MatrixXd tau_tilde(6,6);
Eigen::Vector3d omega(0,0,0);
Eigen::Matrix3d omega_cross;
Eigen::VectorXd a(6);
Eigen::VectorXd b(6);
Eigen::VectorXd z(6);
double C = 0.0;
double CF = 0.0;
double SPOLES = 12.0;
double RPOLES = 14.0;
double epsilon = 0.0;
double mu = 0.0;
/* Recurse from the tip to the base of this chain */
for (int i = N-1; i >= 0; --i)
{
module_link& cur = c.get_link(i);
/* Rotation from the world to this link */
R_cur = c.get_current_R(i);
/* Vector, in world frame, from inbound joint to outbound joint of
* this link
*/
r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1());
/* Operator to transform spatial vectors from outbound joint
* to the inbound joint
*/
phi = get_body_trans(r_i_ip);
/* Vector from the inbound joint to the center of mass */
r_i_cm = R_cur*(-cur.get_r_im1());
/* Operator to transform spatial vectors from the center of mass
* to the inbound joint
*/
phi_cm = get_body_trans(r_i_cm);
/* Cross product matrix corresponding to the vector from
* the inbound joint to the center of mass
*/
L_oc_tilde = get_cross_mat(r_i_cm);
/* 3x3 Inertia matrix of this link about its inbound joint and wrt the
* world coordinate system.
*
* NOTE: Similarity transform of get_I_cm() because it is wrt
* the module frame
*/
J_o = R_cur*cur.get_I_cm()*R_cur.transpose() - cur.get_mass()*L_oc_tilde*L_oc_tilde;
/* Spatial mass matrix of this link about its inbound joint and wrt the
* world coordinate system.
*
*/
M_cur << J_o, cur.get_mass()*L_oc_tilde,
-cur.get_mass()*L_oc_tilde, cur.get_mass()*Iden;
/* Spatial mass matrix of this link about its cm and wrt the
* world coordinate system.
*
* NOTE: Similarity transform of get_I_cm() because it is wrt
* the module frame
*/
M_cm << R_cur*cur.get_I_cm()*R_cur.transpose(), Zero,
Zero, cur.get_mass()*Iden;
/* Articulated Body Spatial inertia matrix of the links from
* this one all the way to the tip of the chain (accounting for
* articulated body spatial inertia that gets through each joint)
*
* pp is p_cur from the previous (outbound) link. This is the
* zero vector when we are on the farthest outbound link (the tip)
*
* In much the same way that we use a similarity transform to change
* the frame of the inertia matrix, a similarity transform moves
* pp from the previous module's base joint to this module's base joint.
*/
p_cur = phi*pp*phi.transpose() + M_cur;
/* The joint matrix, in the body frame, that maps the change
* in general coordinates relating this link to the next inward link.
*
* It essentially defines what "gets through" a link. IE:
* H_b_frame_star = [0,0,1,0,0,0] means that movements and forces
* in the joint's rotational Z axis get through. This is a single
* DOF rotational joint.
*
* H_b_frame_star = eye(6) means that forces in all 6
* (3 rotational, 3 linear) get through the joint using
* 6 independent general coordinates. This is a
* free 6DOF joint.
*
* H_b_frame_star = [0,0,2,0,0,1] is helical joint that rotates twice
* for every single linear unit moved. It is controlled by a single
* general coordinate.
*
* (NOTE/TODO: Using anything but [0,0,1,0,0,0] breaks the code right now.)
*/
H_b_frame_star = cur.get_joint_matrix().transpose();
/* To transform a spatial vector (6 x 1) from one coordinate
* system to another, multiply it by the 6x6 matrix with
* the rotation matrix form one frame to the other on the diagonals
*/
Eigen::MatrixXd tmp_6x6(6,6);
tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), R_cur;
/* Joint matrix in the world frame */
H_w_frame_star = tmp_6x6*H_b_frame_star;
/* As a row vector */
H = H_w_frame_star.transpose();
D = H*p_cur*H.transpose(); /* TODO:Could use H_w_frame_star..but..clarity */
/* TODO: This needs to be changed to D.inverse() for 6DOF
* and D needs to be made a variable sized Eigen Matrix
*/
G = p_cur*H.transpose()*(1.0/D);
/* Articulated body projection operator through this joint. This defines
* which part of the articulated body spatial inertia gets through this joint
*/
tau_tilde = Eigen::MatrixXd::Identity(6,6) - G*H;
/* pp for the next time around the loop */
pp = tau_tilde*p_cur;
omega = c.get_angular_velocity(i);
omega_cross = get_cross_mat(omega);
/* Gyroscopic force */
b << omega_cross*J_o*omega,
cur.get_mass()*omega_cross*omega_cross*r_i_cm;
/* Coriolis and centripetal accel */
a << 0,0,0,
omega_cross*omega_cross*r_i_cm;
/* z is the total spatial force seen by this link at its inward joint
* phi*zp = Force through joint from tip-side module
* b = Gyroscopic force (velocity dependent)
* p_cur*a = Coriolis and Centripetal forces
* phi_cm*M_cm*grav = Force of grav at CM transformed to inbound jt
*/
z = phi*zp + b + p_cur*a + phi_cm*M_cm*grav;
/* Velocity related damping force in the joint */
/* TODO: 6DOF change. epsilon will be an matrix when the joint
* matrix is not a row matrix, so using C in the calculation
* of epsilon will break things.
*/
C = -cur.get_damping()*qd[i];
/* Cogging force which is a function of the joint angle and
* the motor specifics.
*/
CF = cur.get_rotor_cogging()*sin(RPOLES*(q[i]-cur.get_cogging_offset()))
+ cur.get_stator_cogging()*sin(SPOLES*(q[i]-cur.get_cogging_offset()));
/* The "Effective" force through the joint that can actually create
* accelerations. IE: Forces that are in directions in which the joint
* can actually move.
*/
/* TODO: 6DOF change. Epsilon will not be 1x1 for a 6DOF joint, so
* both T[i] and C used here as they are will break things.
*/
epsilon = T[i] + C + CF - H*z;
mu = (1/D)*epsilon; /* 6DOF change: D will be a matrix, need to invert it */
zp = z + G*epsilon;
/* base_tip_step needs G, a, and mu. So store them in the chain object.*/
mu_all[i] = mu;
int cur_index=0;
for (int k=(6*i); k <= (6*(i+1)-1); ++k,++cur_index)
{
G_all[k] = G[cur_index];
a_all[k] = a[cur_index];
}
}
}
