void CX3DTextureTransformNode::print(int indent)
{
FILE *fp = CX3DParser::getDebugLogFp();
char *nodeName = getNodeName();
if (nodeName)
{
float x, y;
CX3DParser::printIndent(indent);
fprintf(fp, "%s (%s)\n", nodeName, CX3DNode::getNodeTypeString(getNodeType()));
getCenter()->getValue(x, y);
CX3DParser::printIndent(indent+1);
fprintf(fp, "center : (%f %f)\n", x, y);
CX3DParser::printIndent(indent+1);
fprintf(fp, "rotation : %f\n", getRotation()->getValue());
getScale()->getValue(x, y);
CX3DParser::printIndent(indent+1);
fprintf(fp, "scale : (%f %f)\n", x, y);
getTranslation()->getValue(x, y);
CX3DParser::printIndent(indent+1);
fprintf(fp, "translation : (%f %f)\n", x, y);
}
}
void evaluateTransform ()
{
if (getType() == TRANSFORM_PRIMITIVE) {
m_contained.translate(getTranslation());
m_contained.rotate(getRotation());
}
}
void StepbackRecovery::runBehavior() {
if(!initialized_) {
ROS_ERROR("This object must be initialized before runBehavior is called");
return;
}
if(global_costmap_ == NULL || local_costmap_ == NULL) {
ROS_ERROR("The costmaps passed to the StepbackRecovery object cannot be NULL. Doing nothing.");
return;
}
ROS_WARN("Stepback recovery behavior started.");
//Get initial position
tf::StampedTransform transform;
tf_->lookupTransform("/odom","/base_footprint",ros::Time(0), transform);
geometry_msgs::TransformStamped msg;
tf::transformStampedTFToMsg(transform, msg);
double x0 = msg.transform.translation.x;
double y0 = msg.transform.translation.y;
ros::Rate r(frequency_);
ros::NodeHandle n;
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
bool stepped_back = false;
while(n.ok()) {
//compute the distance left to move
double dist_left = stepback_length_ - getTranslation(x0, y0);
//compute the velocity that will let us stop by the time we reach the goal
double vel = sqrt(acc_lim_th_ * dist_left);
//make sure that this velocity falls within the specified limits
vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = -vel;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0;
vel_pub.publish(cmd_vel);
//Check if already moved back the whole distance
ROS_DEBUG("Stepback behavior: Distance left %f meters", dist_left);
if(dist_left < 0.0)
stepped_back = true;
//if we're done with our stepping back... then return
if(stepped_back)
return;
r.sleep();
}
}
void VerletCapsuleShape::setTranslation(const glm::vec3& position) {
// NOTE: this method will update the verlet points, which is probably not
// what you want to do. Only call this method if you know what you're doing.
// update the points such that their center is at position
glm::vec3 movement = position - getTranslation();
_startPoint->_position += movement;
_endPoint->_position += movement;
}
// virtual
void VerletCapsuleShape::setHalfHeight(float halfHeight) {
// push points along axis so they are 2*halfHeight apart
glm::vec3 center = getTranslation();
glm::vec3 axis;
computeNormalizedAxis(axis);
_startPoint->_position = center - halfHeight * axis;
_endPoint->_position = center + halfHeight * axis;
_boundingRadius = _radius + halfHeight;
}
void TransformNode::outputContext(ostream &printStream, const char *indentString)
{
float vec[3];
float rot[4];
getTranslation(vec); printStream << indentString << "\t" << "translation " << vec[0] << " "<< vec[1] << " " << vec[2] << endl;
getRotation(rot); printStream << indentString << "\t" << "rotation " << rot[0] << " "<< rot[1] << " " << rot[2] << " " << rot[3] << endl;
getScale(vec); printStream << indentString << "\t" << "scale " << vec[0] << " "<< vec[1] << " " << vec[2] << endl;
getScaleOrientation(rot); printStream << indentString << "\t" << "scaleOrientation " << rot[0] << " "<< rot[1] << " " << rot[2] << " " << rot[3] << endl;
getCenter(vec); printStream << indentString << "\t" << "center " << vec[0] << " "<< vec[1] << " " << vec[2] << endl;
}
//____________________________________________________________________________
void Camera::moveCamera(float speed)
{
// Get the current view vector (the direction we are looking)
Vector3D vector = * this->getLookAtPosition() - * getTranslation();
// I snuck this change in here! We now normalize our view vector when
// moving throughout the world. This is a MUST that needs to be done.
// That way you don't move faster than you strafe, since the strafe vector
// is normalized too.
// vector = Vector3D::getNormalVector(vector);
vector = Vector3D::Normalize(vector);
// setze Kameraposition
getTranslation()->x = getTranslation()->x + vector.x * speed;
getTranslation()->z = getTranslation()->z + vector.z * speed;
// setze Blickpunkt
this->getLookAtPosition()->x = this->getLookAtPosition()->x + vector.x * speed;
this->getLookAtPosition()->z = this->getLookAtPosition()->z + vector.z * speed;
}
void Wikipedia::setEntry(const QString &nEntry)
{
if (nEntry != m_entry) {
m_entry = nEntry;
#ifdef QT_DEBUG
qDebug() << "Changed entry to" << m_entry;
#endif
emit entryChanged(entry());
getTranslation();
}
}
void Wikipedia::setLangCode(const QString &nLangCode)
{
if (nLangCode != m_langCode) {
m_langCode = nLangCode;
#ifdef QT_DEBUG
qDebug() << "Changed langCode to" << m_langCode;
#endif
emit langCodeChanged(langCode());
getTranslation();
}
}
Transform TransformationBuilder::getTransform(const Mat& homography, const vector<Point2f>& objectCorners, const Size2f& imageSize)
{
vector<Point2f> objectCornersTransformed;
perspectiveTransform(objectCorners, objectCornersTransformed, homography);
Transform result;
result.translation = getTranslation(objectCornersTransformed, imageSize);
result.scale = getScale(objectCornersTransformed, objectCorners);
result.rotation = getRotation(homography);
return result;
}
void Camera::setRotation(int mouseX, int mouseY, int * winSize)
{
// Mittelpunkt des Fensters mit binärem Shift ermitteln
int middleX = winSize[0] >> 1; // x / 2 = middle x
int middleY = winSize[1] >> 1;
// Winkel für oben/unten Achse 0.5 = 180 Grad Drehung
float angleY = 0.0f;
// Winkel für rechts/links Achse
float angleZ = 0.0f;
static float currentRotX = 0.0f;
// wenn die Maus im Mittelpunkt ist, hat sich nichts getan => ENDE
if ((mouseX == middleX) && (mouseY == middleY)) return;
// setze den Mauszeiger auf den Mittelpunkt des Fensters
glutWarpPointer(middleX, middleY);
// Differenz zwischen Mittelpunkt und Mausposition ermitteln und
// auf eine Wert zwischen 0 und 1 bringen.
// TODO: Teiler für Steuerung der Mausempfindlichkeit nutzen
angleY = (float)((middleX - mouseX)) / 1000.0f;
angleZ = (float)((middleY - mouseY)) / 1000.0f;
// Zwischenspeichern der z Rotation
// Here we keep track of the current rotation (for up and down) so that
// we can restrict the camera from doing a full 360 loop.
currentRotX -= angleZ;
// If the current rotation (in radians) is greater than 1.0, we want to cap it.
if (currentRotX > 1.0f) currentRotX = 1.0f;
// Check if the rotation is below -1.0, if so we want to make sure it doesn't continue
else if (currentRotX < -1.0f) currentRotX = -1.0f;
// Otherwise, we can rotate the view around our position
else
{
// Um den Vektor für die Drehung um die z - Achse zu ermitteln
// wird das Kreuzprodukt des Kamerarichtungsvektor = Blickpunkt - Position
// und des Normalenvektors der Kamera gebildet.
Vector3D temp = * this->getLookAtPosition() - * getTranslation();
// Vector3D zAxis = Vector3D::cross(& temp, upVector);
Vector3D zAxis = Vector3D::Cross(temp, *upVector);
// zAxis = Vector3D::getNormalVector(zAxis);
zAxis = Vector3D::Normalize(zAxis);
// Rotation um die beiden Achsen
this->rotateView(angleZ, zAxis.x, zAxis.y, zAxis.z);
this->rotateView(angleY, 0, 1, 0); // yAxis
// Ausgabe der aktuellen Positionen
this->getTranslation()->print("\ngetTranslation() : ");
this->getLookAtPosition()->print("lookAtPosition : ");
}
}
void VerletCapsuleShape::setRotation(const glm::quat& rotation) {
// NOTE: this method will update the verlet points, which is probably not
// what you want to do. Only call this method if you know what you're doing.
// update points such that they have the same center but a different axis
glm::vec3 center = getTranslation();
float halfHeight = getHalfHeight();
glm::vec3 axis = rotation * DEFAULT_CAPSULE_AXIS;
_startPoint->_position = center - halfHeight * axis;
_endPoint->_position = center + halfHeight * axis;
}
OSG_USING_NAMESPACE
/*! \class osg::ComponentTransform
*/
/*-------------------------------------------------------------------------*/
/* Changed */
void ComponentTransform::changed(BitVector whichField, UInt32 origin)
{
ComponentTransformPtr ptr(*this);
if((whichField & CenterFieldMask ) ||
(whichField & RotationFieldMask ) ||
(whichField & ScaleFieldMask ) ||
(whichField & ScaleOrientationFieldMask) ||
(whichField & TranslationFieldMask ) )
{
// be careful not to mark the matrix as changed here to avoid
// bouncing changes back and forth
_sfMatrix.getValue().setTransform(getTranslation (),
getRotation (),
getScale (),
getScaleOrientation(),
getCenter () );
invalidateVolume();
}
else if(whichField & Inherited::MatrixFieldMask)
{
Vec3f translation;
Quaternion rotation;
Vec3f scale;
Quaternion scaleOrientation;
Vec3f center;
_sfMatrix.getValue().getTransform(translation,
rotation,
scale,
scaleOrientation,
center );
// be careful not to mark the components as changed here to avoid
// bouncing changes back and forth
_sfTranslation .setValue(translation );
_sfRotation .setValue(rotation );
_sfScale .setValue(scale );
_sfScaleOrientation.setValue(scaleOrientation);
_sfCenter .setValue(center );
}
Inherited::changed(whichField, origin);
}
// Called within Model::simulate call, below.
void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
if (_owningAvatar->isMyAvatar()) {
_rig->computeMotionAnimationState(deltaTime, _owningAvatar->getPosition(), _owningAvatar->getVelocity(), _owningAvatar->getOrientation());
}
Model::updateRig(deltaTime, parentTransform);
if (_owningAvatar->isMyAvatar()) {
const FBXGeometry& geometry = _geometry->getFBXGeometry();
Head* head = _owningAvatar->getHead();
Rig::HeadParameters params;
params.modelRotation = getRotation();
params.modelTranslation = getTranslation();
params.leanSideways = head->getFinalLeanSideways();
params.leanForward = head->getFinalLeanForward();
params.torsoTwist = head->getTorsoTwist();
params.localHeadOrientation = head->getFinalOrientationInLocalFrame();
params.worldHeadOrientation = head->getFinalOrientationInWorldFrame();
params.eyeLookAt = head->getLookAtPosition();
params.eyeSaccade = head->getSaccade();
params.leanJointIndex = geometry.leanJointIndex;
params.neckJointIndex = geometry.neckJointIndex;
params.leftEyeJointIndex = geometry.leftEyeJointIndex;
params.rightEyeJointIndex = geometry.rightEyeJointIndex;
_rig->updateFromHeadParameters(params);
} else {
// This is a little more work than we really want.
//
// Other avatars joint, including their eyes, should already be set just like any other joints
// from the wire data. But when looking at me, we want the eyes to use the corrected lookAt.
//
// Thus this should really only be ... else if (_owningAvatar->getHead()->isLookingAtMe()) {...
// However, in the !isLookingAtMe case, the eyes aren't rotating the way they should right now.
// (They latch their looking at me position.) We will revisit that as priorities allow.
const FBXGeometry& geometry = _geometry->getFBXGeometry();
Head* head = _owningAvatar->getHead();
_rig->updateEyeJoints(geometry.leftEyeJointIndex, geometry.rightEyeJointIndex,
getTranslation(), getRotation(),
head->getFinalOrientationInWorldFrame(), head->getCorrectedLookAtPosition());
}
}
void EclassModelNode::_onTransformationChanged()
{
if (getType() == TRANSFORM_PRIMITIVE)
{
_revertTransform();
translate(getTranslation());
rotate(getRotation());
updateTransform();
}
}
void EclassModelNode::_applyTransformation()
{
if (getType() == TRANSFORM_PRIMITIVE)
{
_revertTransform();
translate(getTranslation());
rotate(getRotation());
_freezeTransform();
}
}
void EclassModelNode::_applyTransformation()
{
if (getType() == TRANSFORM_PRIMITIVE)
{
m_contained.revertTransform();
m_contained.translate(getTranslation());
m_contained.rotate(getRotation());
m_contained.freezeTransform();
}
}
void Synthoid::facePlayer()
{
const Vec4& T = getTranslation();
const Vec4& P = Player::Instance()->GetViewPoint();
Vec4 D = P - T;
D[2] = 0.0f;
D.normalizeIfNotZero();
m_rotation = - radToDeg * atan2f( D[0], D[1] );
m_displayObject->setRotation(m_rotation + m_rotationOffset);
}
void EclassModelNode::_onTransformationChanged()
{
if (getType() == TRANSFORM_PRIMITIVE)
{
m_contained.revertTransform();
m_contained.translate(getTranslation());
m_contained.rotate(getRotation());
m_contained.updateTransform();
}
}
void evaluateTransform()
{
if(getType() == TRANSFORM_PRIMITIVE)
{
m_contained.translate(getTranslation());
m_contained.rotate(getRotation());
}
else
{
transformComponents(calculateTransform());
}
}
/*
* \brief Composition operator
*/
Transform Transform::operator*(const Transform& other) const {
Transform result;
// matrix product
result.a[0] = a[0] * other.a[0] + a[1] * other.a[3];
result.a[1] = a[0] * other.a[1] + a[1] * other.a[4];
result.a[3] = a[3] * other.a[0] + a[4] * other.a[3];
result.a[4] = a[3] * other.a[1] + a[4] * other.a[4];
// operation
Point composed = this->operator()(getTranslation());
result.a[2] = composed.x();
result.a[5] = composed.y();
return result;
}
void Doom3GroupNode::transformComponents(const Matrix4& matrix) {
if (m_curveNURBS.isSelected()) {
m_curveNURBS.transform(matrix);
}
if (m_curveCatmullRom.isSelected()) {
m_curveCatmullRom.transform(matrix);
}
if (_originInstance.isSelected()) {
m_contained.translateOrigin(getTranslation());
}
}
void GameObject::setSentinelRelativeDirection()
{
Sentinel* S = Game::pWorld->pSentinel;
if (S)
{
const Vec4& Ts = S->getTranslation();
const Vec4& T = getTranslation();
m_sentinelRelativeDirection = T - Ts;
m_sentinelRelativeDirection[2] = 0.f;
m_sentinelRelativeDirection.normalizeIfNotZero();
}
}
void QSpaceNavigatorClient::update()
{
SpaceNavigatorClient::update();
if (_unconsumedData)
{
_unconsumedData = false;
double x, y, z, rx, ry, rz;
getTranslation(x, y, z);
getRotation(rx, ry, rz);
//emit updated(x, y, z, rx, ry, rz);
emit translated(x, y, z);
}
}
vgm::MatrixR Transform::gethMatrix() const
{
vgm::MatrixR matrix;
matrix.setTransform(
getTranslation(),
getRotation(),
getScaleFactor(),
getScaleOrientation(),
getCenter()
);
return ( matrix );
}
void CX3DHAnimSiteNode::print(int indent)
{
FILE *fp = CX3DParser::getDebugLogFp();
char *nodeName = getNodeName();
if (nodeName)
{
float x, y, z, rot;
int i, n;
CX3DParser::printIndent(indent);
fprintf(fp, "%s (%s)\n", nodeName, CX3DNode::getNodeTypeString(getNodeType()));
CX3DParser::printIndent(indent+1);
fprintf(fp, "children\n");
MFNode *nodes = getChildren();
n = nodes->count();
for (i=0; i<n; i++)
{
CX3DNode *child = nodes->getNode(i);
if (child)
{
child->print(indent+2);
}
}
CX3DParser::printIndent(indent+1);
getCenter()->getValue(x, y, z);
fprintf(fp, "center : (%f %f %f)\n", x, y, z);
CX3DParser::printIndent(indent+1);
getRotation()->getValue(x, y, z, rot);
fprintf(fp, "rotation : (%f %f %f)(%f)\n", x, y, z, rot);
CX3DParser::printIndent(indent+1);
getScale()->getValue(x, y, z);
fprintf(fp, "scale : (%f %f %f)\n", x, y, z);
CX3DParser::printIndent(indent+1);
getScaleOrientation()->getValue(x, y, z, rot);
fprintf(fp, "scaleOrientation : (%f %f %f)(%f)\n", x, y, z, rot);
CX3DParser::printIndent(indent+1);
getTranslation()->getValue(x, y, z);
fprintf(fp, "translation : (%f %f %f)\n", x, y, z);
CX3DParser::printIndent(indent+1);
fprintf(fp, "name (%s)\n", m_name.getValue());
}
}
TranslationTable::MachineInstructionVector
TranslationTable::translateInstruction(
const ir::Instruction* instruction) const
{
auto translation = getTranslation(instruction->opcodeString());
if(translation == nullptr)
{
// Fail the translation by returning nothing
return MachineInstructionVector();
}
return translation->translateInstruction(instruction);
}
void Doom3GroupNode::transformComponents(const Matrix4& matrix)
{
if (_nurbsEditInstance.isSelected()) {
_nurbsEditInstance.transform(matrix);
}
if (_catmullRomEditInstance.isSelected()) {
_catmullRomEditInstance.transform(matrix);
}
if (_originInstance.isSelected()) {
_d3Group.translateOrigin(getTranslation());
}
}
void TransformNode::getSFMatrix(SFMatrix *mOut)
{
float center[3];
float rotation[4];
float scale[3];
float scaleOri[4];
float trans[3];
SFMatrix mSRI;
SFMatrix mSR;
SFMatrix mCI;
SFMatrix mC;
SFMatrix mT;
SFMatrix mR;
SFMatrix mS;
getTranslation(trans);
mT.setTranslation(trans);
getCenter(center);
mC.setTranslation(center);
getRotation(rotation);
mR.setRotation(rotation);
getScaleOrientation(scaleOri);
mSR.setRotation(scaleOri);
getScale(scale);
mS.setScaling(scale);
getScaleOrientation(scaleOri);
scaleOri[3] = -scaleOri[3];
mSRI.setRotation(scaleOri);
getCenter(center);
center[0] = -center[0];
center[1] = -center[1];
center[2] = -center[2];
mCI.setTranslation(center);
mOut->init();
mOut->add(&mT);
mOut->add(&mC);
mOut->add(&mR);
mOut->add(&mSR);
mOut->add(&mS);
mOut->add(&mSRI);
mOut->add(&mCI);
}
void LocalePreferences::saveValues() {
int dateFormat = getDateFormatNo();
int timeFormat = getTimeFormatNo();
QString translation = getTranslation();
global.settings->beginGroup(INI_GROUP_LOCALE);
global.settings->setValue("translation", translation);
global.settings->setValue("dateFormat", dateFormat);
global.settings->setValue("timeFormat", timeFormat);
global.settings->endGroup();
global.setDateFormat(dateFormat);
global.setTimeFormat(timeFormat);
}