/*!
Sets this quaternion by converting from Inventor format.
*/
void
Quaternion::set(const SbRotation &SbRot)
{
w = (double)SbRot.getValue()[3];
x = (double)SbRot.getValue()[0];
y = (double)SbRot.getValue()[1];
z = (double)SbRot.getValue()[2];
normalise();
}
void Cone::transform(SoTransform *transformation){
scale_vector = transformation->scaleFactor.getValue();
radius = scale_vector[0];
SbRotation rotation = transformation->rotation.getValue();
rotation.getValue(rotation_axis, rotation_angle);
translation_vector = transformation->translation.getValue();
SbMatrix T,S, R, F;
T.setTranslate(translation_vector);
print_vector(translation_vector); //ntc
print_vector(scale_vector); //ntc
//std::cout << rotation[0] << rotation[1]<<rotation[2]<<rotation[3]; //ntc
print_vector(rotation_axis); //ntc
std::cout<<rotation_angle<<std::endl<<std::endl;
R.setRotate(rotation);
S.setScale(scale_vector);
// F = T*R*S;
F = S * R *T;
SbVec3f pos(0,0,0);
F.multVecMatrix(pos, pos);
M = F; // save transformation matrix
if(M.det4() != 0 ){
iM = F.inverse(); // store inverse if inverse exists
}
position = pos;
print_vector(position);
}
void
SoBillboard::getMatrix(SoGetMatrixAction *action)
//
////////////////////////////////////////////////////////////////////////
{
//if (! rotation.isIgnored() && ! rotation.isDefault()) {
SbRotation rot = calculateRotation(action->getState());
SbMatrix &ctm = action->getMatrix();
SbMatrix &inv = action->getInverse();
SbMatrix m;
rot.getValue(m);
ctm.multLeft(m);
rot.invert();
rot.getValue(m);
inv.multRight(m);
//}
}
SbRotation
SoBillboard::calculateRotation(SoState *state)
{
SbRotation rot;
#ifdef INVENTORRENDERER
const SbViewVolume &viewVolume = SoViewVolumeElement::get(state);
if (SbVec3f(0.0f, 0.0f, 0.0f) == axis.getValue())
{
rot = viewVolume.getAlignRotation();
}
#else
const SbMatrix &mm = SoModelMatrixElement::get(state);
SbMatrix imm = mm.inverse();
SbVec3f toviewer;
SbVec3f cameray(0.0f, 1.0f, 0.0f);
const SbViewVolume &vv = SoViewVolumeElement::get(state);
toviewer = -vv.getProjectionDirection();
imm.multDirMatrix(toviewer, toviewer);
(void)toviewer.normalize();
SbVec3f rotaxis = this->axis.getValue();
if (rotaxis == SbVec3f(0.0f, 0.0f, 0.0f))
{
// 1. Compute the billboard-to-viewer vector.
// 2. Rotate the Z-axis of the billboard to be collinear with the
// billboard-to-viewer vector and pointing towards the viewer's position.
// 3. Rotate the Y-axis of the billboard to be parallel and oriented in the
// same direction as the Y-axis of the viewer.
rot.setValue(SbVec3f(0.f, 0.0f, 1.0f), toviewer);
SbVec3f viewup = vv.getViewUp();
imm.multDirMatrix(viewup, viewup);
SbVec3f yaxis(0.0f, 1.0f, 0.0f);
rot.multVec(yaxis, yaxis);
SbRotation rot2(yaxis, viewup);
SbVec3f axis;
float angle;
rot.getValue(axis, angle);
rot2.getValue(axis, angle);
rot = rot * rot2;
//SoModelMatrixElement::rotateBy(state, (SoNode*) this, rot);
}
#endif
else
{
fprintf(stderr, "SoBillboard: axis != (0.0, 0.0, 0.0) not implemented\n");
}
return rot;
}
void
SoModelMatrixElement::rotateEltBy(const SbRotation &rotation)
//
////////////////////////////////////////////////////////////////////////
{
SbMatrix m;
rotation.getValue(m);
modelMatrix.multLeft(m);
flags.isModelIdentity = FALSE; // Assume the worst
flags.haveModelCull = FALSE;
}
bool Save_camera_settings_to_file::
init( std::string& parameters, GsTL_project* proj,
Error_messages_handler* errors ) {
std::vector< std::string > params =
String_Op::decompose_string( parameters, Actions::separator,
Actions::unique );
if( params.size() != 1 ) {
errors->report( "save_camera_settings filename" );
return false;
}
SmartPtr<Named_interface> view_ni =
Root::instance()->interface( projectViews_manager + "/main_view" );
Oinv_view* view = dynamic_cast<Oinv_view*>( view_ni.raw_ptr() );
if( !view ) {
return false;
}
SoCamera* camera = view->get_render_area()->getCamera();
if( !camera ) return false;
std::ofstream out( params[0].c_str() );
if( !out ) {
std::ostringstream message;
message << "Can't write to file " << params[0];
errors->report( message.str() );
return false;
}
SbVec3f pos = camera->position.getValue();
SbRotation rot = camera->orientation.getValue();
float rot1, rot2, rot3, rot4;
rot.getValue( rot1, rot2, rot3, rot4 );
out << pos[0] << " " << pos[1] << " " << pos[2] << "\n"
<< rot1 << " " << rot2 << " " << rot3 << " " << rot4 << "\n"
<< camera->aspectRatio.getValue() << "\n"
<< camera->nearDistance.getValue() << "\n"
<< camera->farDistance.getValue() << "\n"
<< camera->focalDistance.getValue() << std::endl;
out.close();
return true;
}
void ViewProviderRobotObject::DraggerMotionCallback(SoDragger *dragger)
{
float q0, q1, q2, q3;
Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
Base::Placement Tcp = robObj->Tcp.getValue();
const SbMatrix & M = dragger->getMotionMatrix ();
SbVec3f translation;
SbRotation rotation;
SbVec3f scaleFactor;
SbRotation scaleOrientation;
SbVec3f center(Tcp.getPosition().x,Tcp.getPosition().y,Tcp.getPosition().z);
M.getTransform(translation, rotation, scaleFactor, scaleOrientation);
rotation.getValue(q0, q1, q2, q3);
//Base::Console().Message("M %f %f %f\n", M[3][0], M[3][1], M[3][2]);
Base::Rotation rot(q0, q1, q2, q3);
Base::Vector3d pos(translation[0],translation[1],translation[2]);
robObj->Tcp.setValue(Base::Placement(pos,rot));
}
// Write SbRotation to output stream. Used from SoSFRotation and
// SoMFRotation.
void
sosfrotation_write_value(SoOutput * out, const SbRotation & r)
{
SbVec3f axis;
float angle;
r.getValue(axis, angle);
// Handle invalid rotation specifications.
if (axis.length() == 0.0f) {
axis.setValue(0.0f, 0.0f, 1.0f);
angle = 0.0f;
}
out->write(axis[0]);
if(!out->isBinary()) out->write(' ');
out->write(axis[1]);
if(!out->isBinary()) out->write(' ');
out->write(axis[2]);
if(!out->isBinary()) out->write(" ");
out->write(angle);
}
// internal callback
void InvPlaneMover::dragFinishCB(void *me, SoDragger *drag)
{
InvPlaneMover *mee = static_cast<InvPlaneMover *>(me);
if (mee->show_)
{
SbVec3f t = ((SoJackDragger *)drag)->translation.getValue();
int i;
for (i = 0; i < 3; ++i)
t[i] *= mee->scale_->scaleFactor.getValue()[i];
SbRotation r = ((SoJackDragger *)drag)->rotation.getValue();
SbVec3f n;
SbVec3f ax;
float angle;
r.getValue(ax, angle);
SbVec3f axN;
mee->fullRot_->rotation.getValue().multVec(ax, axN);
r.setValue(axN, angle);
r.multVec(mee->nnn_, n);
// we have to rotate the translation around the x-axis
// (because we have a y-axis dragger)
SbVec3f tt;
n.normalize();
// snap normal to the closest coordinate axis
// here done by snaping it to the axis with the biggest projection onto it.
if (mee->motionMode_ == InvPlaneMover::SNAP)
{
int axis;
float mmax;
int dir = 1;
SbVec3f nn;
if (n[0] * n[0] < n[1] * n[1])
{
axis = 1;
mmax = n[1];
if (n[1] < 0)
dir = -1;
else
dir = +1;
//dir = (int) copysign(1,n[1]);
}
else
{
axis = 0;
mmax = n[0];
if (n[0] < 0)
dir = -1;
else
dir = +1;
//dir = (int) copysign(1,n[0]);
}
if (mmax * mmax < n[2] * n[2])
{
axis = 2;
if (n[2] < 0)
dir = -1;
else
dir = +1;
//dir = (int) copysign(1,n[2]);
}
switch (axis)
{
case 0:
nn.setValue(1, 0, 0);
break;
case 1:
nn.setValue(0, 1, 0);
break;
case 2:
nn.setValue(0, 0, 1);
break;
}
n = dir * nn;
}
tt = t[1] * n;
float d;
d = n.dot(tt + mee->distOffset_);
float data[4];
data[0] = n[0];
data[1] = n[1];
data[2] = n[2];
data[3] = d;
// send feedback message to contoller
((InvPlaneMover *)me)->sendFeedback(data);
}
}
void
SoXipNeheStarGenerator::GLRender(SoGLRenderAction *action)
{
float yawAngle = 0;
static float spinAngle = 0;
float distance = 0;
float pitchAngle = M_PI /2; // tilt the view
SbRotation pitch(SbVec3f(1, 0, 0), pitchAngle); // rotation around X
SbMatrix pitchM;
pitch.getValue(pitchM);
SbRotation yaw = SbRotation::identity();
SbMatrix yawM = SbMatrix::identity();
SbRotation spin = SbRotation::identity();
SbMatrix spinM = SbMatrix::identity();
for (int i = 0; i < MAX_STARS; i++)
{
SoXipNeheStar* star = static_cast<SoXipNeheStar*>(this->getChild(i));
// spin angle for this star
spin.setValue(SbVec3f(0,0,1), spinAngle); // rotation around Z
spin.getValue(spinM);
// yaw angle for this star
//_starInfos[i].angle += ((float(i)/MAX_STARS) * (180 / M_PI));
yawAngle = _starInfos[i].angle;
yaw.setValue(SbVec3f(0,1,0), yawAngle); // rotation around Y
yaw.getValue(yawM);
// let's compose all matrices once to position each star
SbMatrix transM = SbMatrix::identity();
transM.setTranslate(SbVec3f(_starInfos[i].distance,0,0));
SbMatrix transform = spinM * pitchM.inverse() * yawM.inverse() * transM * yawM * pitchM ;
// position the star
star->trans.setValue(transform);
unsigned int color = convertRGBtoHex(_starInfos[i].r, _starInfos[i].g, _starInfos[i].b);
star->color.set1Value(0, color);
star->color.set1Value(1, color);
star->color.set1Value(2, color);
star->color.set1Value(3, color);
spinAngle += ( 0.01f * (M_PI / 180));
// change setting of all stars except the very first one (index 0)
if (i)
{
_starInfos[i].angle += ((float(i)/MAX_STARS) * ( M_PI / 180));
_starInfos[i].distance -= 0.01f;
if (_starInfos[i].distance < 0.0f)
{
_starInfos[i].distance += 5.0f;
_starInfos[i].r= rand() % 255 + 1 ;
_starInfos[i].g= rand() % 255 + 1;
_starInfos[i].b= rand() % 255 + 1;
}
}
}
SoXipKit::GLRender(action);
}
SbRotation
SbSphereSectionProjector::getRotation(const SbVec3f &p1, const SbVec3f &p2)
//
////////////////////////////////////////////////////////////////////////
{
SbBool tol1 = isWithinTolerance(p1);
SbBool tol2 = isWithinTolerance(p2);
if (tol1 && tol2) {
// both points in tolerance, rotate about
// sphere center
return SbRotation(
p1 - sphere.getCenter(),
p2 - sphere.getCenter());
}
else if (!tol1 && !tol2) {
// both points out of tolerance, rotate about
// plane point
// Would like to just use this:
SbRotation badRot = SbRotation(p1 - planePoint, p2 - planePoint);
// but fp instablity gives back a goofy axis, so we don't get
// pure roll.
// So we need to snap the axis to be parallel to plane dir
SbVec3f badAxis; float goodAngle;
badRot.getValue(badAxis, goodAngle);
SbVec3f goodAxis;
if (badAxis.dot(planeDir) > 0.0)
goodAxis = planeDir;
else
goodAxis = -planeDir;
SbRotation rollRot(goodAxis, goodAngle);
//Now find rotation in the direction perpendicular to this:
SbVec3f diff1 = p1 - planePoint;
SbVec3f diff2 = p2 - planePoint;
float d = diff2.length() - diff1.length();
// Check for degenerate cases
float theta = d / sphere.getRadius();
if ( fabs(theta) < 0.000001 || fabs(theta) > 1.0 )
return rollRot;
diff1.normalize();
SbVec3f pullAxis = planeDir.cross( diff1 );
pullAxis.normalize();
SbRotation pullRot(pullAxis, getRadialFactor() * theta );
SbRotation totalRot = rollRot * pullRot;
return totalRot;
}
else {
// one point in, one point out, so rotate about
// the center of the sphere from the point on the
// sphere to the intersection of the plane and the
// sphere closest to the point off the sphere
SbLine planeLine;
SbVec3f intersection;
if (tol1) {
planeLine.setValue(planePoint, p2);
}
else {
planeLine.setValue(planePoint, p1);
}
if (! sphere.intersect(planeLine, intersection))
#ifdef DEBUG
SoDebugError::post("SbSphereSectionProjector::getRotation",
"Couldn't intersect plane line with sphere");
#else
/* Do nothing */;
#endif
if (tol1) {
// went off sphere
return SbRotation(
p1 - sphere.getCenter(),
intersection - sphere.getCenter());
}
else {
// came on to sphere
// "Hey cutie. You've got quite a radius..."
return SbRotation(
intersection - sphere.getCenter(),
p2 - sphere.getCenter());
}
}
}
SbMatrix IVElement::orientationMatrix() {
SbMatrix mat;
SbRotation rr = rotVec->getValue();
rr.getValue(mat);
return mat.transpose();
}
void
SbMatrix::setRotate(const SbRotation &rotation)
{
rotation.getValue(*this);
}
void printQ(SbRotation rot)
{
float q0,q1,q2,q3;
rot.getValue(q0,q1,q2,q3);
cout << q0 << " " << q1 << " " << q2 << " " << q3 << " " << endl;
}
