int main()
{
int T;
scanf("%d", &T);
assert(1 <= T && T <= MAXT);
for (int cas = 1; cas <= T; ++cas) {
printf("Testing case %d ..\n", cas);
scanf("%lf", &t);
assert(0.0 + EPS < t && t < MAXVAL - EPS);
bool Sgn = yp(t) > 0.0;
for (double x = t - DELTA, X = t + DELTA; x < X; x += DELTAINC) {
double deriv = yp(x);
bool sgn = deriv > 0.0;
assert(! eps_equal(deriv, 0.0));
assert(sgn == Sgn);
}
}
puts("OK");
return 0;
}
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() ) );
}
data__ operator()(xit__ x, yit__ yb, yit__ ye) const
{
xit__ xp(x);
yit__ y(yb), yp(yb);
data__ rtnval(0);
for (++x, ++y; y!=ye; ++x, ++xp, ++y, ++yp)
rtnval+=((*x)-(*xp))*((*y)+(*yp))/2;
return rtnval;
}
void game_tick( GameThreadSockets & gsockets, GameState & gs, SharedRenderState & srs, unsigned int now ) {
Ogre::Quaternion o;
uint8_t b;
zstr_send( gsockets.zmq_input_req, "mouse_state" );
char * mouse_state = zstr_recv( gsockets.zmq_input_req );
parse_mouse_state( mouse_state, o, b );
free( mouse_state );
uint8_t w, a, s, d, spc, alt;
zstr_send( gsockets.zmq_input_req, "kb_state" );
char * kb_state = zstr_recv( gsockets.zmq_input_req );
parse_kb_state( kb_state, w, a, s, d, spc, alt );
free( kb_state );
// yaw 0 looks towards -Z
float yaw = o.getYaw().valueRadians();
Ogre::Vector3 xp( cosf( yaw ), 0.0, -sinf( yaw ) );
Ogre::Vector3 yp( -sinf( yaw ), 0.0, -cosf( yaw ) );
float speed = 10.0f;
if ( w ) {
srs.position += speed * yp;
}
if ( s ) {
srs.position -= speed * yp;
}
if ( a ) {
srs.position -= speed * xp;
}
if ( d ) {
srs.position += speed * xp;
}
if ( spc ) {
srs.position[1] += speed;
}
if ( alt ) {
srs.position[1] -= speed;
}
}
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 );
}
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();
}
}
vpHomogeneousMatrix DoorHandleDetectionNode::createTFPlane(const vpColVector coeffs, const double x, const double y, const double z)
{
vpColVector xp(3);
vpColVector yp(3);
vpColVector normal(3);
vpRotationMatrix dRp;
vpTranslationVector P0;
vpHomogeneousMatrix dMp;
geometry_msgs::Pose dMp_msg;
tf::Transform transform;
static tf::TransformBroadcaster br;
//Create a normal to the plan from the coefficients
normal[0] = -coeffs[0];
normal[1] = -coeffs[1];
normal[2] = -coeffs[2];
normal.normalize();
//Create a xp vector that is following the equation of the plane
xp[0] = - (coeffs[1]*y + coeffs[2]*(z+0.05) + coeffs[3]) / (coeffs[0]) - x;
xp[1] = 0;
xp[2] = 0.05;
xp.normalize();
//Create a yp vector with the normal and xp
yp = vpColVector::cross(normal,xp);
//Create the Rotation Matrix
dRp[0][0] = xp[0];
dRp[1][0] = xp[1];
dRp[2][0] = xp[2];
dRp[0][1] = yp[0];
dRp[1][1] = yp[1];
dRp[2][1] = yp[2];
dRp[0][2] = normal[0];
dRp[1][2] = normal[1];
dRp[2][2] = normal[2];
transform.setOrigin( tf::Vector3(x, y, z) );
//Calculate the z0 for the translation vector
double z0 = -(coeffs[0]*x + coeffs[1]*y + coeffs[3])/(coeffs[2]);
//Create the translation Vector
P0 = vpTranslationVector(x, y, z0);
//Create the homogeneous Matrix
dMp = vpHomogeneousMatrix(P0, dRp);
//Publish the tf of the plane
dMp_msg = visp_bridge::toGeometryMsgsPose(dMp);
tf::Quaternion q;
q.setX(dMp_msg.orientation.x);
q.setY(dMp_msg.orientation.y);
q.setZ(dMp_msg.orientation.z);
q.setW(dMp_msg.orientation.w);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), m_parent_depth_tf, "tf_plane"));
return dMp;
}
/*
Calculate interpolation coefficients for Monotone splines
*/
Matrix<double> setupSplineMonotoneSpline(const Vector<double>& x,
const Vector<double>& y) {
// these are the interpolation coefficients
// y(x, x_i-1<x<x_i) = _ai (x - x_i)^3 + _bi (x - x_i)^2
// + _ci (x - x_i) + _di
const unsigned int n = x.size();
Matrix<double> interp_coeffs(4, n - 1);
// initialize x-grid step sizes and slopes.
Vector<double> h(n - 1);
Vector<double> s(n - 1);
for (unsigned int i = 0; i < n - 1; ++i) {
h(i) = x(i + 1) - x(i);
s(i) = (y(i + 1) - y(i)) / h(i);
}
if (n > 2) {
Vector<double> p(n);
p(0) = 0.0;
p(n - 1) = 0.0;
for (unsigned int i = 1; i < n - 1; ++i)
p(i) = (s(i - 1) * h(i) + s(i) * h(i - 1)) / (h(i - 1) + h(i));
Vector<double> yp(n);
for (unsigned int i = 1; i < n - 1; ++i) {
double s1 = abs(s(i - 1));
double s2 = abs(s(i));
if (s(i - 1) * s(i) <= 0.0) {
yp(i) = 0.0;
} else if ((abs(p(i)) > 2.0 * s1) ||
(abs(p(i)) > 2.0 * s2)) {
double a = (s1 > 0.0) - (s1 < 0.0); //sign function.
yp(i) = 2.0 * a * min(s1, s2);
} else {
yp(i) = p(i);
}
}
// second derivative at extreme points equal
// to zero boundary condition.
yp(0) = 1.5 * s(0) - 0.5 * yp(1);
yp(n - 1) = 1.5 * s(n - 2) - 0.5 * yp(n - 3);
for (unsigned int i = 0; i < n - 1; ++i) {
interp_coeffs(3, i) = (yp(i) + yp(i + 1) - 2.0 * s(i))
/ (h(i) * h(i));
interp_coeffs(2, i) = (3.0 * s(i) - 2.0 * yp(i) - yp(i + 1))
/ h(i);
interp_coeffs(1, i) = yp(i);
interp_coeffs(0, i) = y(i);
}
} else {
// in this case, with second derivative at extreme points equal to zero,
// the solution will be just the linear interpolation.
interp_coeffs(3, 0) = 0.0;
interp_coeffs(3, 1) = 0.0;
interp_coeffs(2, 0) = 0.0;
interp_coeffs(2, 1) = 0.0;
interp_coeffs(1, 0) = s(0);
interp_coeffs(1, 1) = s(1);
interp_coeffs(0, 0) = y(0);
interp_coeffs(0, 1) = y(1);
}
return interp_coeffs;
}
void RungeKuttaFehlberg::Integrate
(void (*Model)(double t, const Vector &y, Vector &yp),
Vector &y,
double &t, double &tout,
double relerr, double abserr,
int &iflag, int maxnfe)
{
// Get machine epsilon
const double eps = DBL_EPSILON,
u26 = 26*eps;
// remin is the minimum acceptable value of relerr. Attempts
// to obtain higher accuracy with this subroutine are usually
// very expensive and often unsuccessful.
const double remin = 1e-12;
// The expense is controlled by restricting the number
// of function evaluations to be approximately maxnfe.
// The default value of maxnfe = 3000 corresponds to about 500 steps.
int mflag = abs(iflag);
// input check parameters
if (y.Elements() < 1 || relerr < 0.0 || abserr < 0.0 || mflag == 0
|| mflag > InvalidParameters
|| ((t == tout) && (kflag != SmallRelErrorBound))) {
iflag = InvalidParameters;
return;
}
double dt,rer,scale,hmin,eeoet,ee,et,esttol,s,ae,tol = 0;
int output, hfaild, k;
// references for convenience
Vector &yp = Work[0],
&f1 = Work[1],
&ss = Work[norder+1];
int lo = y.Lo(), hi = y.Hi();
int gflag = 0;
if (iflag == SmallRelErrorBound
|| (mflag == NormalStep && (init == 0 || kflag == NormalStep))) {
gflag = Start;
goto next;
}
if (iflag == TooManyIterations
|| (kflag == TooManyIterations && mflag == NormalStep)) {
nfe = 0;
if (mflag != NormalStep) gflag = Start;
goto next;
}
if ((kflag == SmallAbsErrorBound && abserr == 0.0)
|| (kflag == MinimumStepReached && relerr < savre && abserr < savae)) {
iflag = UnsolvableProblem;
return;
}
next:
if (gflag) {
iflag = jflag;
if (kflag == SmallRelErrorBound) mflag = abs(iflag);
}
jflag = iflag;
kflag = 0;
savre = relerr;
savae = abserr;
// Restrict relative error tolerance to be at least as large as
// 2*eps+remin to avoid limiting precision difficulties arising
// from impossible accuracy requests
rer = 2 * eps + remin;
// Relative error tolerance is too small
if (relerr < rer) {
relerr = rer;
iflag = kflag = SmallRelErrorBound;
return;
}
gflag = 0;
dt = tout - t;
if (mflag == Start) {
// Initialization
// Set initialization completion indicator, init
// Set indicator for too many output points,kop
// Evaluate initial derivatives
// Set counter for function evaluations,nfe
// Estimate starting stepsize
init = 0;
kop = 0;
gflag = Start;
Model(t,y,yp); // call function
nfe = 1;
if (t == tout) {
iflag = NormalStep;
return;
}
}
if (init == 0 || gflag) {
init = 1;
h = fabs(dt);
double ypk;
for (int k = lo; k <= hi; k++) {
tol = relerr * fabs(y(k)) + abserr;
if (tol > 0.0) {
ypk = fabs(yp(k));
if (ypk * pow(h,order) > tol)
h = pow(tol/ypk,iorder);
}
}
if (tol <= 0.0) h = 0.0;
ypk = MpMax(fabs(dt),fabs(t));
h = MpMax(h, u26 * ypk);
jflag = CopySign((int)NormalStep,iflag);
}
// Set stepsize for integration in the direction from t to tout
h = CopySign(h,dt);
// Test to see if this routine is being severely impacted by too many
// output points
if (fabs(h) >= 2*fabs(dt)) kop++;
if (kop == 100) {
kop = 0;
iflag = TooManyCalls;
return;
}
if (fabs(dt) <= u26 * fabs(t)) {
// If too close to output point,extrapolate and return
for (int k = lo; k <= hi; k++)
y(k) += dt * yp(k);
Model(tout,y,yp);
nfe++;
t = tout;
iflag = NormalStep;
return;
}
// Initialize output point indicator
output = false;
// To avoid premature underflow in the error tolerance function,
// scale the error tolerances
scale = 2.0 / relerr;
ae = scale * abserr;
// Step by step integration - as an endless loop over steps
for (;;) {
hfaild = 0;
// Set smallest allowable stepsize
hmin = u26 * fabs(t);
// Adjust stepsize if necessary to hit the output point.
// Look ahead two steps to avoid drastic changes in the stepsize and
// thus lessen the impact of output points on the code.
dt = tout - t;
if (fabs(dt) < 2 * fabs(h)) {
if (fabs(dt) <= fabs(h)) {
// The next successful step will complete the
// integration to the output point
output = true;
h = dt;
} else
h = 0.5 * dt;
}
// Core integrator for taking a single step
//
// The tolerances have been scaled to avoid premature underflow in
// computing the error tolerance function et.
// To avoid problems with zero crossings, relative error is measured
// using the average of the magnitudes of the solution at the
// beginning and end of a step.
// The error estimate formula has been grouped to control loss of
// significance.
// To distinguish the various arguments, h is not permitted
// to become smaller than 26 units of roundoff in t.
// Practical limits on the change in the stepsize are enforced to
// smooth the stepsize selection process and to avoid excessive
// chattering on problems having discontinuities.
// To prevent unnecessary failures, the code uses 9/10 the stepsize
// it estimates will succeed.
// After a step failure, the stepsize is not allowed to increase for
// the next attempted step. This makes the code more efficient on
// problems having discontinuities and more effective in general
// since local extrapolation is being used and extra caution seems
// warranted.
//
// Test number of derivative function evaluations.
// If okay,try to advance the integration from t to t+h
if (nfe > maxnfe) {
// Too much work
iflag = kflag = TooManyIterations;
return;
}
step:
// Advance an approximate solution over one step of length h
RungeKuttaStep(Model,y,t,h,ss);
for (int i = lo; i <= hi; i++) f1(i) = ss(i);
nfe += norder;
// Compute and test allowable tolerances versus local error estimates
// and remove scaling of tolerances. note that relative error is
// measured with respect to the average of the magnitudes of the
// solution at the beginning and end of the step.
eeoet = 0.0;
for (k = lo; k <= hi; k++) {
et = fabs(y(k)) + fabs(f1(k)) + ae;
// Inappropriate error tolerance
if (et <= 0.0) {
iflag = SmallAbsErrorBound;
return;
}
ee = fabs( ErrorTerm(k) );
eeoet = MpMax(eeoet,ee/et);
}
esttol = fabs(h) * eeoet * scale;
if (esttol > 1.0) {
// Unsuccessful step
// Reduce the stepsize , try again
// The decrease is limited to a factor of 1/10
hfaild = true;
output = false;
s = 0.1;
if (esttol < crit) s = 0.9 / pow(esttol,iorder);
h *= s;
if (fabs(h) > hmin) goto step; // loop
// Requested error unattainable at smallest allowable stepsize
iflag = kflag = MinimumStepReached;
return;
}
// Successful step
// Store solution at t+h and evaluate derivatives there
t += h;
for (k = lo; k <= hi; k++) y(k) = f1(k);
Model(t,y,yp);
nfe++;
// Choose next stepsize
// The increase is limited to a factor of 5
// if step failure has just occurred, next
// stepsize is not allowed to increase
s = 5.0;
if (esttol > criti) s = 0.9 / pow(esttol,iorder);
if (hfaild) s = MpMin(1.0,s);
h = CopySign(MpMax(hmin,s*fabs(h)),h);
// End of core integrator
if (output) {
t = tout;
iflag = NormalStep;
return;
}
if (iflag <= 0) {
// one-step mode
iflag = SingleStep;
return;
}
} // for (;;)
}
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;
}
clsparseStatus
cg(cldenseVectorPrivate *pX,
const clsparseCsrMatrixPrivate* pA,
const cldenseVectorPrivate *pB,
PTYPE& M,
clSParseSolverControl solverControl,
clsparseControl control)
{
assert( pA->num_cols == pB->num_values );
assert( pA->num_rows == pX->num_values );
if( ( pA->num_cols != pB->num_values ) || ( pA->num_rows != pX->num_values ) )
{
return clsparseInvalidSystemSize;
}
//opaque input parameters with clsparse::array type;
clsparse::vector<T> x(control, pX->values, pX->num_values);
clsparse::vector<T> b(control, pB->values, pB->num_values);
cl_int status;
T scalarOne = 1;
T scalarZero = 0;
//clsparse::vector<T> norm_b(control, 1, 0, CL_MEM_WRITE_ONLY, true);
clsparse::scalar<T> norm_b(control, 0, CL_MEM_WRITE_ONLY, false);
//norm of rhs of equation
status = Norm1<T>(norm_b, b, control);
CLSPARSE_V(status, "Norm B Failed");
//norm_b is calculated once
T h_norm_b = norm_b[0];
#ifndef NDEBUG
std::cout << "norm_b " << h_norm_b << std::endl;
#endif
if (h_norm_b == 0) //special case b is zero so solution is x = 0
{
solverControl->nIters = 0;
solverControl->absoluteTolerance = 0.0;
solverControl->relativeTolerance = 0.0;
//we can either fill the x with zeros or cpy b to x;
x = b;
return clsparseSuccess;
}
//continuing "normal" execution of cg algorithm
const auto N = pA->num_cols;
//helper containers, all need to be zeroed
clsparse::vector<T> y(control, N, 0, CL_MEM_READ_WRITE, true);
clsparse::vector<T> z(control, N, 0, CL_MEM_READ_WRITE, true);
clsparse::vector<T> r(control, N, 0, CL_MEM_READ_WRITE, true);
clsparse::vector<T> p(control, N, 0, CL_MEM_READ_WRITE, true);
clsparse::scalar<T> one(control, 1, CL_MEM_READ_ONLY, true);
clsparse::scalar<T> zero(control, 0, CL_MEM_READ_ONLY, true);
// y = A*x
status = csrmv<T>(one, pA, x, zero, y, control);
CLSPARSE_V(status, "csrmv Failed");
//r = b - y
status = r.sub(b, y, control);
//status = elementwise_transform<T, EW_MINUS>(r, b, y, control);
CLSPARSE_V(status, "b - y Failed");
clsparse::scalar<T> norm_r(control, 0, CL_MEM_WRITE_ONLY, false);
status = Norm1<T>(norm_r, r, control);
CLSPARSE_V(status, "norm r Failed");
//T residuum = 0;
clsparse::scalar<T> residuum(control, 0, CL_MEM_WRITE_ONLY, false);
//residuum = norm_r[0] / h_norm_b;
residuum.div(norm_r, norm_b, control);
solverControl->initialResidual = residuum[0];
#ifndef NDEBUG
std::cout << "initial residuum = "
<< solverControl->initialResidual << std::endl;
#endif
if (solverControl->finished(solverControl->initialResidual))
{
solverControl->nIters = 0;
return clsparseSuccess;
}
//apply preconditioner z = M*r
M(r, z, control);
//copy inital z to p
p = z;
//rz = <r, z>, here actually should be conjugate(r)) but we do not support complex type.
clsparse::scalar<T> rz(control, 0, CL_MEM_WRITE_ONLY, false);
status = dot<T>(rz, r, z, control);
CLSPARSE_V(status, "<r, z> Failed");
int iteration = 0;
bool converged = false;
clsparse::scalar<T> alpha (control, 0, CL_MEM_READ_WRITE, false);
clsparse::scalar<T> beta (control, 0, CL_MEM_READ_WRITE, false);
//yp buffer for inner product of y and p vectors;
clsparse::scalar<T> yp(control, 0, CL_MEM_WRITE_ONLY, false);
clsparse::scalar<T> rz_old(control, 0, CL_MEM_WRITE_ONLY, false);
while(!converged)
{
solverControl->nIters = iteration;
//y = A*p
status = csrmv<T>(one, pA, p, zero, y, control);
CLSPARSE_V(status, "csrmv Failed");
status = dot<T>(yp, y, p, control);
CLSPARSE_V(status, "<y,p> Failed");
// alpha = <r,z> / <y,p>
//alpha[0] = rz[0] / yp[0];
alpha.div(rz, yp, control);
#ifndef NDEBUG
std::cout << "alpha = " << alpha[0] << std::endl;
#endif
//x = x + alpha*p
status = axpy<T>(x, alpha, p, x, control);
CLSPARSE_V(status, "x = x + alpha * p Failed");
//r = r - alpha * y;
status = axpy<T, EW_MINUS>(r, alpha, y, r, control);
CLSPARSE_V(status, "r = r - alpha * y Failed");
//apply preconditioner z = M*r
M(r, z, control);
//store old value of rz
//improve that by move or swap
rz_old = rz;
//rz = <r,z>
status = dot<T>(rz, r, z, control);
CLSPARSE_V(status, "<r,z> Failed");
// beta = <r^(i), r^(i)>/<r^(i-1),r^(i-1)> // i: iteration index;
// beta is ratio of dot product in current iteration compared
//beta[0] = rz[0] / rz_old[0];
beta.div(rz, rz_old, control);
#ifndef NDEBUG
std::cout << "beta = " << beta[0] << std::endl;
#endif
//p = z + beta*p;
status = axpby<T>(p, one, z, beta, p, control );
CLSPARSE_V(status, "p = z + beta*p Failed");
//calculate norm of r
status = Norm1<T>(norm_r, r, control);
CLSPARSE_V(status, "norm r Failed");
//residuum = norm_r[0] / h_norm_b;
status = residuum.div(norm_r, norm_b, control);
CLSPARSE_V(status, "residuum");
iteration++;
converged = solverControl->finished(residuum[0]);
solverControl->print();
}
return clsparseSuccess;
}
