ViewWidget::ViewWidget(QWidget * widget, osgViewer::View * view, osg::Group * sceneRoot, bool showGrids, bool showAxes)
: _widget(widget),
_view(view),
compassAxes(0),
mpXYGridTransform(0),
mpXZGridTransform(0),
mpYZGridTransform(0)
{
if (!view)
return;
_sceneRoot = static_cast<osg::Group*>(_view->getSceneData());
_showGrids = showGrids;
_showAxes = showAxes;
if (_showGrids)
{
initGrids();
if (mpXYGridTransform && sceneRoot)
_sceneRoot->addChild(getXYGrid());
}
if (_showAxes)
{
initAxes();
if (compassAxes && sceneRoot)
_sceneRoot->addChild(getAxes());
}
xyGridToggled = showGrids;
axesToggled = showAxes;
}
void
dxJointUniversal::computeInitialRelativeRotations()
{
if ( node[0].body )
{
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross;
getAxes( ax1, ax2 );
// Axis 1.
dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
dRtoQ( R, qcross );
dQMultiply1( qrel1, node[0].body->q, qcross );
// Axis 2.
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] );
dRtoQ( R, qcross );
if ( node[1].body )
{
dQMultiply1( qrel2, node[1].body->q, qcross );
}
else
{
// set joint->qrel to qcross
for ( int i = 0; i < 4; i++ ) qrel2[i] = qcross[i];
}
}
}
ViewWidget::~ViewWidget()
{
if (showAxes() && _sceneRoot)
_sceneRoot->removeChild(getAxes());
if (showGrids() && _sceneRoot)
_sceneRoot->removeChild(getXYGrid());
delete _widget;
delete compassAxes;
}
bool CollisionSystem::intersects(sf::VertexArray aBounds, sf::VertexArray bBounds)
{
for(sf::Vector2f& axis : getAxes(aBounds))
{
sf::Vector2f proj1 = getProjection(aBounds, axis);
sf::Vector2f proj2 = getProjection(bBounds, axis);
//If they DON'T overlap, a separating axis was found! No collision!
if(!(proj1.x < proj2.y && proj1.y > proj2.x))
return false;
}
for(sf::Vector2f& axis : getAxes(bBounds))
{
sf::Vector2f proj1 = getProjection(aBounds, axis);
sf::Vector2f proj2 = getProjection(bBounds, axis);
//If they DON'T overlap, a separating axis was found! No collision!
if(!(proj1.x < proj2.y && proj1.y > proj2.x))
return false;
}
return true;
}
/**
Get the tranform tensor for vtkTensorGlyph
@param ellipticalShape: ellipsoidal parameters.
@param peak: peak under consideration.
*/
std::array<float, 9> vtkPeakMarkerFactory::getTransformTensor(
const Mantid::DataObjects::PeakShapeEllipsoid &ellipticalShape,
const IPeak &peak) const {
std::vector<double> radii = ellipticalShape.abcRadii();
std::vector<Mantid::Kernel::V3D> axes = getAxes(ellipticalShape, peak);
// The rotation+scaling matrix is the
// principal axes of the ellipsoid scaled by the radii.
std::array<float, 9> tensor;
for (unsigned j = 0; j < 3; ++j) {
for (unsigned k = 0; k < 3; ++k) {
tensor[3 * j + k] = static_cast<float>(radii[j] * axes[j][k]);
}
}
return tensor;
}
dReal
dxJointUniversal::getAngle2()
{
if ( node[0].body )
{
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross, qq, qrel;
getAxes( ax1, ax2 );
// It should be possible to get both angles without explicitly
// constructing the rotation matrix of the cross. Basically,
// orientation of the cross about axis1 comes from body 2,
// about axis 2 comes from body 1, and the perpendicular
// axis can come from the two bodies somehow. (We don't really
// want to assume it's 90 degrees, because in general the
// constraints won't be perfectly satisfied, or even very well
// satisfied.)
//
// However, we'd need a version of getHingeAngleFromRElativeQuat()
// that CAN handle when its relative quat is rotated along a direction
// other than the given axis. What I have here works,
// although it's probably much slower than need be.
dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] );
dRtoQ( R, qcross );
if ( node[1].body )
{
dQMultiply1( qq, node[1].body->q, qcross );
dQMultiply2( qrel, qq, qrel2 );
}
else
{
// pretend joint->node[1].body->q is the identity
dQMultiply2( qrel, qcross, qrel2 );
}
return - getHingeAngleFromRelativeQuat( qrel, axis2 );
}
return 0;
}
void
dxJointPU::getInfo2( dxJoint::Info2 *info )
{
const int s0 = 0;
const int s1 = info->rowskip;
const int s2 = 2 * s1;
const dReal k = info->fps * info->erp;
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos1, *pos2 = 0, *R1, *R2 = 0;
pos1 = node[0].body->posr.pos;
R1 = node[0].body->posr.R;
if ( node[1].body )
{
pos2 = node[1].body->posr.pos;
R2 = node[1].body->posr.R;
}
dVector3 axP; // Axis of the prismatic joint in global frame
dMultiply0_331( axP, R1, axisP1 );
// distance between the body1 and the anchor2 in global frame
// Calculated in the same way as the offset
dVector3 dist;
dVector3 wanchor2 = {0,0,0};
if ( node[1].body )
{
dMultiply0_331( wanchor2, R2, anchor2 );
dist[0] = wanchor2[0] + pos2[0] - pos1[0];
dist[1] = wanchor2[1] + pos2[1] - pos1[1];
dist[2] = wanchor2[2] + pos2[2] - pos1[2];
}
else
{
if (flags & dJOINT_REVERSE )
{
// Invert the sign of dist
dist[0] = pos1[0] - anchor2[0];
dist[1] = pos1[1] - anchor2[1];
dist[2] = pos1[2] - anchor2[2];
}
else
{
dist[0] = anchor2[0] - pos1[0];
dist[1] = anchor2[1] - pos1[1];
dist[2] = anchor2[2] - pos1[2];
}
}
dVector3 q; // Temporary axis vector
// Will be used at 2 places with 2 different meaning
// ======================================================================
// Work on the angular part (i.e. row 0)
//
// The axis perpendicular to both axis1 and axis2 should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the rotoide axes should be equal. Thus the constraint equations are
// p*w1 - p*w2 = 0
// where p is a unit vector perpendicular to both axis1 and axis2
// and w1 and w2 are the angular velocity vectors of the two bodies.
dVector3 ax1, ax2;
getAxes( ax1, ax2 );
dReal val = dCalcVectorDot3( ax1, ax2 );
q[0] = ax2[0] - val * ax1[0];
q[1] = ax2[1] - val * ax1[1];
q[2] = ax2[2] - val * ax1[2];
dVector3 p;
dCalcVectorCross3( p, ax1, q );
dNormalize3( p );
// info->J1a[s0+i] = p[i];
dCopyVector3(( info->J1a ) + s0, p );
if ( node[1].body )
{
// info->J2a[s0+i] = -p[i];
dCopyNegatedVector3(( info->J2a ) + s0, p );
}
// compute the right hand side of the constraint equation. Set relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
// = (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,
// theta - Pi/2 ~= cos(theta), so
// |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
info->c[0] = k * - val;
// ==========================================================================
// Work on the linear part (i.e rows 1 and 2)
//
// We want: vel2 = vel1 + w1 x c ... but this would
// result in three equations, so we project along the planespace vectors
// so that sliding along the axisP is disregarded.
//
// p1 + R1 dist' = p2 + R2 anchor2'
// v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'
// v_p is speed of prismatic joint (i.e. elongation rate)
// Since the constraints are perpendicular to v_p we have:
// e1 dot v_p = 0 and e2 dot v_p = 0
// e1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
// e2 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
// ==
// e1 . v1 + e1 . w1 x dist = e1 . v2 + e1 . w2 x anchor2
// since a . (b x c) = - b . (a x c) = - (a x c) . b
// and a x b = - b x a
// e1 . v1 - e1 x dist . w1 - e1 . v2 - (- e1 x anchor2 . w2) = 0
// e1 . v1 + dist x e1 . w1 - e1 . v2 - anchor2 x e1 . w2 = 0
// Coeff for 1er line of: J1l => e1, J2l => -e1
// Coeff for 2er line of: J1l => e2, J2l => -ax2
// Coeff for 1er line of: J1a => dist x e1, J2a => - anchor2 x e1
// Coeff for 2er line of: J1a => dist x e2, J2a => - anchor2 x e2
// e1 and e2 are perpendicular to axP
// so e1 = ax1 and e2 = ax1 x axP
// N.B. ax2 is not always perpendicular to axP since it is attached to body 2
dCalcVectorCross3( q , ax1, axP );
dMultiply0_331( axP, R1, axisP1 );
dCalcVectorCross3(( info->J1a ) + s1, dist, ax1 );
dCalcVectorCross3(( info->J1a ) + s2, dist, q );
// info->J1l[s1+i] = ax[i];
dCopyVector3(( info->J1l ) + s1, ax1 );
// info->J1l[s2+i] = q[i];
dCopyVector3(( info->J1l ) + s2, q );
if ( node[1].body )
{
// Calculate anchor2 in world coordinate
// q x anchor2 instead of anchor2 x q since we want the negative value
dCalcVectorCross3(( info->J2a ) + s1, ax1, wanchor2 );
// The cross product is in reverse order since we want the negative value
dCalcVectorCross3(( info->J2a ) + s2, q, wanchor2 );
// info->J2l[s1+i] = -ax1[i];
dCopyNegatedVector3(( info->J2l ) + s1, ax1 );
// info->J2l[s2+i] = -ax1[i];
dCopyNegatedVector3(( info->J2l ) + s2, q );
}
// We want to make correction for motion not in the line of the axisP
// We calculate the displacement w.r.t. the anchor pt.
//
// compute the elements 1 and 2 of right hand side.
// We want to align the offset point (in body 2's frame) with the center of body 1.
// The position should be the same when we are not along the prismatic axis
dVector3 err;
dMultiply0_331( err, R1, anchor1 );
// err[i] = dist[i] - err[i];
dSubtractVectors3( err, dist, err );
info->c[1] = k * dCalcVectorDot3( ax1, err );
info->c[2] = k * dCalcVectorDot3( q, err );
int row = 3 + limot1.addLimot( this, info, 3, ax1, 1 );
row += limot2.addLimot( this, info, row, ax2, 1 );
if ( node[1].body || !(flags & dJOINT_REVERSE) )
limotP.addLimot( this, info, row, axP, 0 );
else
{
axP[0] = -axP[0];
axP[1] = -axP[1];
axP[2] = -axP[2];
limotP.addLimot ( this, info, row, axP, 0 );
}
}
void
dxJointUniversal::getAngles( dReal *angle1, dReal *angle2 )
{
if ( node[0].body )
{
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross, qq, qrel;
getAxes( ax1, ax2 );
// It should be possible to get both angles without explicitly
// constructing the rotation matrix of the cross. Basically,
// orientation of the cross about axis1 comes from body 2,
// about axis 2 comes from body 1, and the perpendicular
// axis can come from the two bodies somehow. (We don't really
// want to assume it's 90 degrees, because in general the
// constraints won't be perfectly satisfied, or even very well
// satisfied.)
//
// However, we'd need a version of getHingeAngleFromRElativeQuat()
// that CAN handle when its relative quat is rotated along a direction
// other than the given axis. What I have here works,
// although it's probably much slower than need be.
dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
dRtoQ( R, qcross );
// This code is essentialy the same as getHingeAngle(), see the comments
// there for details.
// get qrel = relative rotation between node[0] and the cross
dQMultiply1( qq, node[0].body->q, qcross );
dQMultiply2( qrel, qq, qrel1 );
*angle1 = getHingeAngleFromRelativeQuat( qrel, axis1 );
// This is equivalent to
// dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
// You see that the R is constructed from the same 2 axis as for angle1
// but the first and second axis are swapped.
// So we can take the first R and rapply a rotation to it.
// The rotation is around the axis between the 2 axes (ax1 and ax2).
// We do a rotation of 180deg.
dQuaternion qcross2;
// Find the vector between ax1 and ax2 (i.e. in the middle)
// We need to turn around this vector by 180deg
// The 2 axes should be normalize so to find the vector between the 2.
// Add and devide by 2 then normalize or simply normalize
// ax2
// ^
// |
// |
/// *------------> ax1
// We want the vector a 45deg
//
// N.B. We don't need to normalize the ax1 and ax2 since there are
// normalized when we set them.
// We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z]
qrel[0] = 0; // equivalent to cos(Pi/2)
qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1
qrel[2] = ax1[1] + ax2[1];
qrel[3] = ax1[2] + ax2[2];
dReal l = dRecip( sqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] ) );
qrel[1] *= l;
qrel[2] *= l;
qrel[3] *= l;
dQMultiply0( qcross2, qrel, qcross );
if ( node[1].body )
{
dQMultiply1( qq, node[1].body->q, qcross2 );
dQMultiply2( qrel, qq, qrel2 );
}
else
{
// pretend joint->node[1].body->q is the identity
dQMultiply2( qrel, qcross2, qrel2 );
}
*angle2 = - getHingeAngleFromRelativeQuat( qrel, axis2 );
}
else
{
*angle1 = 0;
*angle2 = 0;
}
}
void
dxJointUniversal::getInfo2( dReal worldFPS, dReal worldERP,
int rowskip, dReal *J1, dReal *J2,
int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
int *findex )
{
// set the three ball-and-socket rows
setBall( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 );
// set the universal joint row. the angular velocity about an axis
// perpendicular to both joint axes should be equal. thus the constraint
// equation is
// p*w1 - p*w2 = 0
// where p is a vector normal to both joint axes, and w1 and w2
// are the angular velocity vectors of the two bodies.
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
// length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
// about this.
dVector3 p;
// Since axis1 and axis2 may not be perpendicular
// we find a axis2_tmp which is really perpendicular to axis1
// and in the plane of axis1 and axis2
getAxes( ax1, ax2 );
dReal k = dCalcVectorDot3( ax1, ax2 );
dVector3 ax2_temp;
dAddVectorScaledVector3(ax2_temp, ax2, ax1, -k);
dCalcVectorCross3( p, ax1, ax2_temp );
dNormalize3( p );
int currRowSkip = 3 * rowskip;
{
dCopyVector3( J1 + currRowSkip + GI2__JA_MIN, p);
if ( node[1].body )
{
dCopyNegatedVector3( J2 + currRowSkip + GI2__JA_MIN, p);
}
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
// = (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,
// theta - Pi/2 ~= cos(theta), so
// |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
int currPairSkip = 3 * pairskip;
{
pairRhsCfm[currPairSkip + GI2_RHS] = worldFPS * worldERP * (-k);
}
currRowSkip += rowskip; currPairSkip += pairskip;
// if the first angle is powered, or has joint limits, add in the stuff
if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 ))
{
currRowSkip += rowskip; currPairSkip += pairskip;
}
// if the second angle is powered, or has joint limits, add in more stuff
limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 );
}
void
dxJointUniversal::getInfo2( dxJoint::Info2 *info )
{
// set the three ball-and-socket rows
setBall( this, info, anchor1, anchor2 );
// set the universal joint row. the angular velocity about an axis
// perpendicular to both joint axes should be equal. thus the constraint
// equation is
// p*w1 - p*w2 = 0
// where p is a vector normal to both joint axes, and w1 and w2
// are the angular velocity vectors of the two bodies.
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dVector3 ax2_temp;
// length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
// about this.
dVector3 p;
dReal k;
// Since axis1 and axis2 may not be perpendicular
// we find a axis2_tmp which is really perpendicular to axis1
// and in the plane of axis1 and axis2
getAxes( ax1, ax2 );
k = dCalcVectorDot3( ax1, ax2 );
ax2_temp[0] = ax2[0] - k * ax1[0];
ax2_temp[1] = ax2[1] - k * ax1[1];
ax2_temp[2] = ax2[2] - k * ax1[2];
dCalcVectorCross3( p, ax1, ax2_temp );
dNormalize3( p );
int s3 = 3 * info->rowskip;
info->J1a[s3+0] = p[0];
info->J1a[s3+1] = p[1];
info->J1a[s3+2] = p[2];
if ( node[1].body )
{
info->J2a[s3+0] = -p[0];
info->J2a[s3+1] = -p[1];
info->J2a[s3+2] = -p[2];
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
// = (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,
// theta - Pi/2 ~= cos(theta), so
// |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
info->c[3] = info->fps * info->erp * - k;
// if the first angle is powered, or has joint limits, add in the stuff
int row = 4 + limot1.addLimot( this, info, 4, ax1, 1 );
// if the second angle is powered, or has joint limits, add in more stuff
limot2.addLimot( this, info, row, ax2, 1 );
}
