public: void free(){ for(int iside=0; iside<4; iside++){ Side* side = getSide(iside); if(side != NULL) for(int jrib=0; jrib<4; jrib++){ if(this == side->getRib(jrib)){ side->setRib(NULL, jrib); break; } } } for(int icell=0; icell<4; icell++){ Cell* cell = getCell(icell); if(cell != NULL) for(int jrib=0; jrib<4; jrib++){ if(this == cell->getRib(getAxis(), jrib)){ cell->setRib(getAxis(), jrib, NULL); break; } } } for(int i=0; i<4; i++){ setSide(i, NULL); setCell(i, NULL); } //cout << "delete " << this << endl;; delete this; }
double sphericalBlendShape::getAzimuth(MPoint point, short aPoleAxis, short aSeamAxis) { double azimuth = atan2(getAxis(MVector(point), axisCross(aPoleAxis, aSeamAxis)), getAxis(MVector(point), aSeamAxis)); // atan2 returns the range [-pi, pi]. remap to [0, 2pi). if (azimuth < 0 && isNearZero(azimuth)) { azimuth += 2 * M_PI; } return azimuth; }
void joyCallback(const sensor_msgs::JoyConstPtr& joy) { velocity_.linear.x = getAxis(joy, axes_.x.axis) * axes_.x.max; velocity_.linear.y = getAxis(joy, axes_.y.axis) * axes_.y.max; velocity_.linear.z = getAxis(joy, axes_.z.axis) * axes_.z.max; velocity_.angular.z = getAxis(joy, axes_.yaw.axis) * axes_.yaw.max; if (getButton(joy, buttons_.slow.button)) { velocity_.linear.x *= slow_factor_; velocity_.linear.y *= slow_factor_; velocity_.linear.z *= slow_factor_; velocity_.angular.z *= slow_factor_; } velocity_publisher_.publish(velocity_); }
ZIntCuboidFaceArray ZIntCuboidFace::cropBy(const ZIntCuboidFace &face) const { ZIntCuboidFaceArray faceArray; if (hasOverlap(face)) { if (isWithin(face)) { return faceArray; } else { ZIntCuboidFace subface(getAxis(), isNormalPositive()); subface.setZ(getPlanePosition()); subface.set(getFirstCorner(), Corner(getUpperBound(0), face.getLowerBound(1) - 1)); faceArray.appendValid(subface); subface.set(Corner(getLowerBound(0), imax2(getLowerBound(1), face.getLowerBound(1))), Corner(face.getLowerBound(0) - 1, getUpperBound(1))); faceArray.appendValid(subface); subface.set(Corner(face.getUpperBound(0) + 1, imax2(getLowerBound(1), face.getLowerBound(1))), getLastCorner()); faceArray.appendValid(subface); subface.set(Corner(imax2(getLowerBound(0), face.getLowerBound(0)), face.getUpperBound(1) + 1), Corner(imin2(getUpperBound(0), face.getUpperBound(0)), getUpperBound(1))); faceArray.appendValid(subface); } #if 0 else if (face.isWithin(*this)) { ZIntCuboidFace subface(getAxis(), isNormalPositive()); secondCorner.set(getUpperBound(0), face.getLowerBound(1)); subface.set(getFirstCorner(), secondCorner); faceArray.appendValid(subface); subface.set(Corner(getLowerBound(0), face.getLowerBound(1)), face.getCorner(2)); faceArray.appendValid(subface); subface.set(Corner(getLowerBound(0), face.getUpperBound(1)), getLastCorner()); faceArray.appendValid(subface); } else { } #endif } else {
bool SOrientedBoundingBox::intersects(SOrientedBoundingBox &obb) const { SVector3 collide_axes[15]; for(int i = 0; i < 3; i++) { collide_axes[i] = getAxis(i); collide_axes[i + 3] = obb.getAxis(i); } SVector3 sizes[2]; sizes[0] = getSize(); sizes[1] = obb.getSize(); for(std::size_t i = 0; i < 3; i++) { for(std::size_t j = 3; j < 6; j++) { collide_axes[3 * i + j + 3] = crossprod(collide_axes[i], collide_axes[j]); } } SVector3 T = obb.getCenter() - getCenter(); for(std::size_t i = 0; i < 15; i++) { double val = 0.0; for(std::size_t j = 0; j < 6; j++) { val += 0.5 * (sizes[j < 3 ? 0 : 1])(j % 3) * std::abs(dot(collide_axes[j], collide_axes[i])); } if(std::abs(dot(collide_axes[i], T)) > val) { return false; } } return true; }
void ZIntCuboidFace::print() const { std::cout << "Normal axis: "; if (m_isNormalPositive) { std::cout << "+"; } else { std::cout << "-"; } switch (getAxis()) { case NeuTube::X_AXIS: std::cout << "X"; break; case NeuTube::Y_AXIS: std::cout << "Y"; break; case NeuTube::Z_AXIS: std::cout << "Z"; break; default: break; } std::cout << "(" << m_z << ")"; std::cout << "; "; std::cout << "(" << getFirstCorner().getX() << ", " << getFirstCorner().getY() << ") --> (" << getLastCorner().getX() << ", " << getLastCorner().getY() << ")" << std::endl; }
bool ZIntCuboidFace::hasOverlap(const ZIntCuboidFace &face) const { if (getAxis() != face.getAxis() || getPlanePosition() != face.getPlanePosition()) { return false; } if (getLowerBound(0) > face.getUpperBound(0)) { return false; } if (getLowerBound(1) > face.getUpperBound(1)) { return false; } if (face.getLowerBound(0) > getUpperBound(0)) { return false; } if (face.getLowerBound(1) > getUpperBound(1)) { return false; } return true; }
bool ZIntCuboidFace::contains(int x, int y, int z) const { int u = 0; int v = 0; int w = 0; switch (getAxis()) { case NeuTube::X_AXIS: u = y; v = z; w = x; break; case NeuTube::Y_AXIS: u = x; v = z; w = y; break; case NeuTube::Z_AXIS: u = x; v = y; w = z; break; } if (w != getPlanePosition()) { return false; } return getLowerBound(0) <= u && getUpperBound(0) >= u && getLowerBound(1) <= v && getUpperBound(1) >= v; }
void dJointAddUniversalTorques( dJointID j, dReal torque1, dReal torque2 ) { dxJointUniversal* joint = ( dxJointUniversal* )j; dVector3 axis1, axis2; dAASSERT( joint ); checktype( joint, Universal ); if ( joint->flags & dJOINT_REVERSE ) { dReal temp = torque1; torque1 = - torque2; torque2 = - temp; } getAxis( joint, axis1, joint->axis1 ); getAxis2( joint, axis2, joint->axis2 ); axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; if ( joint->node[0].body != 0 ) dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] ); if ( joint->node[1].body != 0 ) dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] ); }
// compute axis transformations from bone axis angles void Bone::computeLocalAxisTransform() { CHANNEL_TYPE* axis_order = getAxisOrder(); Vector3D axis = getAxis(); C = Matrix4x4::identity(); for (short d=0; d<3; d++) { Matrix4x4 r; switch(axis_order[d]) { case CT_RX: r = Matrix4x4::rotationPitch(axis.pitch); C = r * C; break; case CT_RY: r = Matrix4x4::rotationYaw(axis.yaw); C = r * C; break; case CT_RZ: r = Matrix4x4::rotationRoll(axis.roll); C = r * C; break; default: break; } } Cinv = C.cheapInverse(true); }
void motorSimController::motorSimTask() { epicsTimeStamp now; double delta; int axis; motorSimAxis *pAxis; while ( 1 ) { /* Get a new timestamp */ epicsTimeGetCurrent( &now ); delta = epicsTimeDiffInSeconds( &now, &(prevTime_) ); prevTime_ = now; if ( delta > (DELTA/4.0) && delta <= (4.0*DELTA) ) { /* A reasonable time has elapsed, it's not a time step in the clock */ for (axis=0; axis<numAxes_; axis++) { this->lock(); pAxis = getAxis(axis); pAxis->process(delta ); this->unlock(); } } epicsThreadSleep( DELTA ); } }
//////////////////////////////////////// //! getAxis() //! Override asynMotorController function to return pointer to IMS axis object // //! Returns a pointer to an ImsMDrivePlusAxis object. //! Returns NULL if the axis number encoded in pasynUser is invalid // //! param[in] pasynUser asynUser structure that encodes the axis index number //////////////////////////////////////// ImsMDrivePlusMotorAxis* ImsMDrivePlusMotorController::getAxis(asynUser *pasynUser) { int axisNo; getAddress(pasynUser, &axisNo); return getAxis(axisNo); }
/** Reports on status of the driver * \param[in] fp The file pointer on which report information will be written * \param[in] level The level of report detail desired * * If details > 0 then information is printed about each axis. * After printing controller-specific information calls asynMotorController::report() */ void C300Controller::report(FILE *fp, int level) { int axis; C300Axis *pAxis; fprintf(fp, "C300 motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_); if (level > 0) { for (axis=0; axis<numAxes_; axis++) { pAxis = getAxis(axis); fprintf(fp, " axis %d\n" " bitsPerUnit_ = %f\n" " encoder position=%f\n" " theory position=%f\n" " limits=0x%x\n", pAxis->axisNo_, pAxis->bitsPerUnit_, pAxis->encoderPosition_, pAxis->theoryPosition_, pAxis->currentLimits_); } } // Call the base class method asynMotorController::report(fp, level); }
void dJointGetPRAxis2( dJointID j, dVector3 result ) { dxJointPR* joint = ( dxJointPR* ) j; dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); checktype( joint, PR ); getAxis( joint, result, joint->axisR1 ); }
void dJointGetScrewAxis( dJointID j, dVector3 result ) { dxJointScrew* joint = ( dxJointScrew* )j; dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); checktype( joint, Screw ); getAxis( joint, result, joint->axis1 ); }
void dJointGetHingeAxis( dJointID j, dVector3 result ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); checktype( joint, Hinge ); getAxis( joint, result, joint->axis1 ); }
void dJointGetPUAxis3( dJointID j, dVector3 result ) { dxJointPU* joint = ( dxJointPU* ) j; dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); checktype( joint, PU ); getAxis( joint, result, joint->axisP1 ); }
void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { if (m_useSolveConstraintObsolete) { m_timeStep = timeStep; //calculateTransforms(); int i; // linear btVector3 pointInA = m_calculatedTransformA.getOrigin(); btVector3 pointInB = m_calculatedTransformB.getOrigin(); btScalar jacDiagABInv; btVector3 linear_axis; for (i=0;i<3;i++) { if (m_linearLimits.isLimited(i)) { jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); if (m_useLinearReferenceFrameA) linear_axis = m_calculatedTransformA.getBasis().getColumn(i); else linear_axis = m_calculatedTransformB.getBasis().getColumn(i); m_linearLimits.solveLinearAxis( m_timeStep, jacDiagABInv, m_rbA,bodyA,pointInA, m_rbB,bodyB,pointInB, i,linear_axis, m_AnchorPos); } } // angular btVector3 angular_axis; btScalar angularJacDiagABInv; for (i=0;i<3;i++) { if (m_angularLimits[i].needApplyTorques()) { // get axis angular_axis = getAxis(i); angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB); } } } }
//============================================================================== void PrismaticJoint::updateLocalTransform() const { mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getAxis() * getPositionsStatic()) * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); }
Mantid::API::MatrixWorkspace_sptr getTransmissionWorkspace(NXcanSASTestTransmissionParameters ¶meters) { auto ws = WorkspaceCreationHelper::create1DWorkspaceConstant( parameters.size, parameters.value, parameters.error); ws->setTitle(parameters.name); ws->getAxis(0)->unit() = Mantid::Kernel::UnitFactory::Instance().create("Wavelength"); return ws; }
//============================================================================== void RevoluteJoint::updateRelativeTransform() const { mT = Joint::mAspectProperties.mT_ParentBodyToJoint * math::expAngular(getAxis() * getPositionsStatic()) * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); }
public: void free(){ /* if(isSplitted()){ for(int i=0; i<2; i++){ subribs[i]->free(); } } */ for(int iside=0; iside<4; iside++){ Side* side = getSide(iside); for(int jrib=0; jrib>4; jrib++){ if(side != NULL && this == side->getRib(jrib)){ side->setRib(NULL, jrib); break; } } } for(int icell=0; icell<4; icell++){ Cell* cell = getCell(icell); for(int jrib=0; jrib>4; jrib++){ if(cell != NULL && this == cell->getRib(getAxis(), jrib)){ cell->setRib(getAxis(), jrib, NULL); break; } } } /* if(getParent() != NULL){ for(int i=0; i<2; i++){ if(this == getParent()->getSubRib(i)){ getParent()->setSubRib(i, NULL); break; } } } */ for(int i=0; i<4; i++){ setSide(i, NULL); setCell(i, NULL); } delete this; }
/** Create an EventWorkspace containing fake data * of single-crystal diffraction. * Instrument is MINITOPAZ * * @return EventWorkspace_sptr */ EventWorkspace_sptr createDiffractionEventWorkspace(int numEvents, int numPixels, int numBins) { double binDelta = 10.0; auto retVal = boost::make_shared<EventWorkspace>(); retVal->initialize(numPixels, 1, 1); // --------- Load the instrument ----------- const std::string filename = FileFinder::Instance().getFullPath( "IDFs_for_UNIT_TESTING/MINITOPAZ_Definition.xml"); InstrumentDefinitionParser parser(filename, "MINITOPAZ", Strings::loadFile(filename)); auto instrument = parser.parseXML(nullptr); retVal->populateInstrumentParameters(); retVal->setInstrument(instrument); DateAndTime run_start("2010-01-01T00:00:00"); for (int pix = 0; pix < numPixels; pix++) { for (int i = 0; i < numEvents; i++) { retVal->getSpectrum(pix) += Mantid::DataObjects::TofEvent( (i + 0.5) * binDelta, run_start + double(i)); } retVal->getSpectrum(pix).addDetectorID(pix); } // Create the x-axis for histogramming. HistogramData::BinEdges x1(numBins); auto &xRef = x1.mutableData(); for (int i = 0; i < numBins; ++i) { xRef[i] = i * binDelta; } // Set all the histograms at once. retVal->setAllX(x1); // Default unit: TOF. retVal->getAxis(0)->setUnit("TOF"); // Give it a crystal and goniometer WorkspaceCreationHelper::setGoniometer(retVal, 0., 0., 0.); WorkspaceCreationHelper::setOrientedLattice(retVal, 1., 1., 1.); // Some sanity checks if (retVal->getInstrument()->getName() != "MINITOPAZ") throw std::runtime_error("MDEventsTestHelper::" "createDiffractionEventWorkspace(): Wrong " "instrument loaded."); Mantid::detid2det_map dets; retVal->getInstrument()->getDetectors(dets); if (dets.size() != 100 * 100) throw std::runtime_error("MDEventsTestHelper::" "createDiffractionEventWorkspace(): Wrong " "instrument size."); return retVal; }
// Changes the acceleration of this delay line to the parameter given void MM3000::setAcceleration(long int theAcelaration) { if(theAcelaration > 0) { int curr_axis = getAxis(); //Select which Dln will be changed char command[30]; //Place holder for the command to be sent and output of the command sprintf(command,"%d%s%ld",curr_axis,"ac", theAcelaration); //Write the command "ac" to set acceleration WriteData(command); //Send the command } }
// Changes the velocity of this delay line to the parameter given void MM3000::setVelocity(long int theVelocity) { if(theVelocity > 0) { int curr_axis = getAxis(); //Select which Dln will move char command[30]; //Place holder for the command to be sent and output of the command sprintf(command,"%d%s%ld",curr_axis,"va", theVelocity); //Write the command "va" to set velocity WriteData(command); //Send the command } }
void RotateObjectsToolPage::OnRotate(wxCommandEvent& event) { if (IsBeingDeleted()) return; const Vec3 center = m_tool->rotationCenter(); const Vec3 axis = getAxis(); const FloatType angle = Math::radians(m_angle->GetValue()); MapDocumentSPtr document = lock(m_document); document->rotateObjects(center, axis, angle); }
void dJointGetPUAxis2( dJointID j, dVector3 result ) { dxJointPU* joint = ( dxJointPU* ) j; dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); checktype( joint, PU ); if ( joint->flags & dJOINT_REVERSE ) getAxis( joint, result, joint->axis1 ); else getAxis2( joint, result, joint->axis2 ); }
// Returns the absolute position of the device // The return value is in device units double MM3000::getActualPosition() { int curr_axis = getAxis(); //Used to select axis is being used (currently 2) char command[30], result[30]; //Place holders for the commands to be sent sprintf(command,"%d%s",curr_axis,"tpe"); //Writes the command "tpe" to ask for position in encoder units WriteData(command); //Sends the command to the device Sleep(200); ReadData(30,command); //Returns the information of the position of this DelayLine int value = asciiToInt(command); return value; //Returns the position }
bool OrientedBox3F::isContained( const Point3F& point ) const { Point3F distToCenter = point - getCenter(); for( U32 i = 0; i < 3; ++ i ) { F32 coeff = mDot( distToCenter, getAxis( i ) ); if( mFabs( coeff ) > getHalfExtents()[ i ] ) return false; } return true; }
std::string Photon::toString() const { std::ostringstream oss; oss << "Photon[" << endl << " pos = " << getPosition().toString() << "," << endl << " power = " << getPower().toString() << "," << endl << " direction = " << getDirection().toString() << "," << endl << " normal = " << getNormal().toString() << "," << endl << " axis = " << getAxis() << "," << endl << " depth = " << getDepth() << endl << "]"; return oss.str(); }