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; }