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