// goal = 1/2 sum( wi * ai^2 ) / sum(wi) for WRMS
// ai == rotation angle between sensor and observation (-Pi:Pi)
int OrientationSensors::calcGoal(const State& state, Real& goal) const {
const SimbodyMatterSubsystem& matter = getMatterSubsystem();
goal = 0;
// Loop over each body that has one or more active osensors.
Real wtot = 0;
PerBodyOSensors::const_iterator bodyp = bodiesWithOSensors.begin();
for (; bodyp != bodiesWithOSensors.end(); ++bodyp) {
const MobilizedBodyIndex mobodIx = bodyp->first;
const Array_<OSensorIx>& bodyOSensors = bodyp->second;
const MobilizedBody& mobod = matter.getMobilizedBody(mobodIx);
const Rotation& R_GB = mobod.getBodyRotation(state);
assert(bodyOSensors.size());
// Loop over each osensor on this body.
for (unsigned m=0; m < bodyOSensors.size(); ++m) {
const OSensorIx mx = bodyOSensors[m];
const OSensor& osensor = osensors[mx];
assert(osensor.bodyB == mobodIx); // better be on this body!
const Rotation& R_GO = observations[getObservationIxForOSensor(mx)];
if (R_GO.isFinite()) { // skip NaNs
const Rotation R_GS = R_GB * osensor.orientationInB;
const Rotation R_SO = ~R_GS*R_GO; // error, in S
const Vec4 aa_SO = R_SO.convertRotationToAngleAxis();
goal += osensor.weight * square(aa_SO[0]);
wtot += osensor.weight;
}
}
}
goal /= (2*wtot);
return 0;
}
void MemSpectrum::flatten(Spectrum * spec, const Rotation & rot)
{
assert( spec );
assert( spec->getDimCount() == rot.size() );
assert( rot.size() > 2 );
const int cx = spec->getScale( rot[ DimX ] ).getSampleCount();
const int cy = spec->getScale( rot[ DimY ] ).getSampleCount();
int x,y;
Rotation cut( rot.size() - 2 );
Dimension d;
for( d = 0; d < cut.size(); d++ )
cut[ d ] = rot[ d + 2 ];
Root::Cube cube( rot.size() );
int c;
for( d = DimZ; d < rot.size(); d++ )
{
c = spec->getScale( rot[ d ] ).getSampleCount();
cube[ rot[ d ] ].first = 0;
cube[ rot[ d ] ].second = c - 1;
}
Buffer buf;
for( x = 0; x < cx; x++ )
{
cube[ rot[ DimX ] ].first = cube[ rot[ DimX ] ].second = x;
for( y = 0; y < cy; y++ )
{
cube[ rot[ DimY ] ].first = cube[ rot[ DimY ] ].second = y;
spec->fillBuffer( buf, cut, cube );
d_buf.setAt( x, y, buf.calcMean() );
}
}
}
//-------------------------------------------------------------------
bool testRotationTwoAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2 ) {
// Form rotation about specified axes
Rotation rotationSpecified( bodyOrSpace, angle1, axis1, angle2, axis2 );
// Form equivalent rotation by another means
Rotation AB( angle1, axis1 );
Rotation BC( angle2, axis2 );
Rotation testRotation = (bodyOrSpace == BodyRotationSequence) ? AB * BC : BC * AB;
// Test to see if they are the same
bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
// Do the inverse problem to back out the angles
const Vec2 testVec = rotationSpecified.convertTwoAxesRotationToTwoAngles( bodyOrSpace, axis1, axis2 );
const Real theta1 = testVec[0];
const Real theta2 = testVec[1];
// Create a Rotation matrix with the backed-out angles and compare to the original Rotation matrix
testRotation.setRotationFromTwoAnglesTwoAxes( bodyOrSpace, theta1, axis1, theta2, axis2 );
test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
// Conversion should produce same angles for for appropriate ranges of angle1 and angle2
if( axis1.isSameAxis(axis2) )
test = test && testInverseRotation1Angle( angle1+angle2, theta1+theta2 );
else
test = test && testInverseRotation2Angle( angle1,theta1, angle2,theta2 );
return test;
}
TYPED_TEST(EulerAnglesXyzDiffTest, testFiniteDifference)
{
typedef typename TestFixture::Scalar Scalar;
typedef typename TestFixture::Rotation Rotation;
typedef typename TestFixture::RotationDiff RotationDiff;
typedef typename TestFixture::Vector3 Vector3;
const double dt = 1.0e-3;
for (auto rotation : this->rotations) {
for (auto angularVelocity2 : this->angularVelocities) {
RotationDiff rotationDiff(angularVelocity2.toImplementation());
rot::LocalAngularVelocity<Scalar> angularVelocity(rotation, rotationDiff);
// Finite difference method for checking derivatives
Rotation rotationNext = rotation.boxPlus(dt*rotation.rotate(angularVelocity.vector()));
rotationNext.setUnique();
rotation.setUnique();
Vector3 dn = (rotationNext.toImplementation()-rotation.toImplementation())/dt;
ASSERT_NEAR(rotationDiff.x(),dn(0),1e-3) << " angular velocity: " << angularVelocity << " rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
ASSERT_NEAR(rotationDiff.y(),dn(1),1e-3) << " angular velocity: " << angularVelocity << "rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
ASSERT_NEAR(rotationDiff.z(),dn(2),1e-3) << " angular velocity: " << angularVelocity << " rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
}
}
}
Placement Command::getPlacement (void)
{
std::string x = "X";
std::string y = "Y";
std::string z = "Z";
std::string a = "A";
std::string b = "B";
std::string c = "C";
double xval = 0.0;
double yval = 0.0;
double zval = 0.0;
double aval = 0.0;
double bval = 0.0;
double cval = 0.0;
if (Parameters.count(x))
xval = Parameters[x];
if (Parameters.count(y))
yval = Parameters[y];
if (Parameters.count(z))
zval = Parameters[z];
if (Parameters.count(a))
aval = Parameters[a];
if (Parameters.count(b))
bval = Parameters[b];
if (Parameters.count(c))
cval = Parameters[c];
Vector3d vec(xval,yval,zval);
Rotation rot;
rot.setYawPitchRoll(aval,bval,cval);
Placement plac(vec,rot);
return plac;
}
void Camera::ProjectPoints(const CvMat* object_points, double gl[16], CvMat* image_points) const
{
double glm[4][4] = {
gl[0], gl[4], gl[8], gl[12],
gl[1], gl[5], gl[9], gl[13],
gl[2], gl[6], gl[10], gl[14],
gl[3], gl[7], gl[11], gl[15],
};
CvMat glm_mat = cvMat(4, 4, CV_64F, glm);
// For some reason we need to mirror both y and z ???
double cv_mul_data[4][4];
CvMat cv_mul = cvMat(4, 4, CV_64F, cv_mul_data);
cvSetIdentity(&cv_mul);
cvmSet(&cv_mul, 1, 1, -1);
cvmSet(&cv_mul, 2, 2, -1);
cvMatMul(&cv_mul, &glm_mat, &glm_mat);
// Rotation
Rotation r;
r.SetMatrix(&glm_mat);
double rod[3];
CvMat rod_mat=cvMat(3, 1, CV_64F, rod);
r.GetRodriques(&rod_mat);
// Translation
double tra[3] = { glm[0][3], glm[1][3], glm[2][3] };
CvMat tra_mat = cvMat(3, 1, CV_64F, tra);
// Project points
ProjectPoints(object_points, &rod_mat, &tra_mat, image_points);
}
/**
* Convert angles to direction cosines.
* @param aE1 1st Euler angle.
* @param aE2 2nd Euler angle.
* @param aE3 3rd Euler angle.
* @param rDirCos Vector of direction cosines.
*/
void SimbodyEngine::convertAnglesToDirectionCosines(double aE1, double aE2, double aE3, double rDirCos[3][3]) const
{
Vec3 angs(aE1, aE2, aE3);
Rotation aRot;
aRot.setRotationToBodyFixedXYZ(angs);
Mat33::updAs(&rDirCos[0][0]) = aRot.asMat33();
}
void CameraController::rotateTowardRobot(Vector3d r_pos)
{
m_my->setPosition(m_rotatePos);
// カメラの位置を得る
m_my->getPosition(m_pos);
// カメラの位置からロボットを結ぶベクトル
Vector3d tmpp = r_pos;
tmpp -= m_pos;
// y方向は考えない
tmpp.y(0);
// カメラの回転角度を得る
Rotation myRot;
m_my->getRotation(myRot);
// カメラのy軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
// ロボットまでの回転角度を得る
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
// 角度差から回転量を得る
targetAngle += theta;
m_my->setAxisAndAngle(0, 1, 0, -targetAngle, 0);
}
Actor* addMap()
{
Actor* sphere = new Actor;
TransformComponent* trans = new TransformComponent();
trans->setPos(vec3(0, -0.01, 0));
Rotation rot;
rot.setEulerAngles(vec3(0, 0.785, 0));
trans->setRotation(rot);
sphere->addComponent(trans);
glDebug();
sphere->addComponent(new StaticMeshStaticPhysicsComponent(AssetManager::getBasePath() + "Data/Model/mappa4.obj"));
glDebug();
MeshGraphicComponent* mesh = new MeshGraphicComponent(AssetManager::getBasePath() + "Data/Model/mappa4.obj");
glDebug();
mesh->setTextureMatrix(mat2(scale(mat4(1), vec3(10))));
glDebug();
Material *mat = new Material();
glDebug();
mat->setDiffuse(AssetManager::getBasePath() + "Data/Texture/floor1_d.png");
mat->setNormal(AssetManager::getBasePath() + "Data/Texture/floor1_n.png");
mat->setSpecular(AssetManager::getBasePath() + "Data/Texture/floor1_s.png");
mat->setShininess(20);
glDebug();
mesh->addMaterial(mat);
glDebug();
sphere->addComponent(mesh);
glDebug();
return sphere;
}
Actor* addAxis3()
{
Actor* actor = new Actor;
TransformComponent* trans = new TransformComponent();
trans->setPos(vec3(-5, 0, 5));
Rotation rot;
rot.setNormalDirection(vec3(1, 0, 0));
trans->setRotation(rot);
actor->addComponent(trans);
MeshGraphicComponent* mesh = new MeshGraphicComponent(AssetManager::getBasePath() + "Data/Model/axis.obj");
Material *mat = new Material();
mat->setDiffuse("#FF0000");
mesh->addMaterial(mat);
Material *mat1 = new Material();
mat1->setDiffuse("#00FF00");
mesh->addMaterial(mat1);
mesh->addMaterial(mat1);
Material *mat2 = new Material();
mat2->setDiffuse("#0000FF");
mesh->addMaterial(mat2);
actor->addComponent(mesh);
return actor;
}
/**
* Convert quaternions to direction cosines.
* @param aQ1 1st Quaternion.
* @param aQ2 2nd Quaternion.
* @param aQ3 3rd Quaternion.
* @param aQ4 4th Quaternion.
* @param rDirCos Vector of direction cosines.
*/
void SimbodyEngine::convertQuaternionsToDirectionCosines(double aQ1, double aQ2, double aQ3, double aQ4, double rDirCos[3][3]) const
{
Rotation R;
R.setRotationFromQuaternion(Quaternion(Vec4(aQ1, aQ2, aQ3, aQ4)));
Mat33::updAs(&rDirCos[0][0]) = R.asMat33();
}
void Drawable::SetGLMatTraQuat(double *tra, double *quat, bool flip)
{
Rotation r;
if (quat != 0)
{
CvMat cv_mat;
cvInitMatHeader(&cv_mat, 4, 1, CV_64F, quat);
r.SetQuaternion(&cv_mat);
}
int flp = 1;
if (flip)
{
r.Transpose();
//flp=-1;
}
CvMat cv_gl_mat;
cvInitMatHeader(&cv_gl_mat, 4, 4, CV_64F, gl_mat); cvZero(&cv_gl_mat);
r.GetMatrix(&cv_gl_mat);
cvSet2D(&cv_gl_mat, 0, 3, cvScalar(flp*tra[0]));
cvSet2D(&cv_gl_mat, 1, 3, cvScalar(flp*tra[1]));
cvSet2D(&cv_gl_mat, 2, 3, cvScalar(flp*tra[2]));
cvSet2D(&cv_gl_mat, 3, 3, cvScalar(1));
cvTranspose(&cv_gl_mat, &cv_gl_mat);
}
/**
* Convert quaternions to direction cosines.
* @param aQ1 1st Quaternion.
* @param aQ2 2nd Quaternion.
* @param aQ3 3rd Quaternion.
* @param aQ4 4th Quaternion.
* @param rDirCos Vector of direction cosines.
*/
void SimbodyEngine::convertQuaternionsToDirectionCosines(double aQ1, double aQ2, double aQ3, double aQ4, double *rDirCos) const
{
if(rDirCos==NULL) return;
Rotation R;
R.setRotationFromQuaternion(Quaternion(Vec4(aQ1, aQ2, aQ3, aQ4)));
Mat33::updAs(rDirCos) = R.asMat33();
}
double DemoRobotController::rotateTowardObj(Vector3d pos)
{
// "pos" means target position
// get own position
Vector3d ownPosition;
m_robotObject->getPartsPosition(ownPosition,"RARM_LINK2");
// pointing vector for target
Vector3d l_pos = pos;
l_pos -= ownPosition;
// ignore variation on y-axis
l_pos.y(0);
// get own rotation matrix
Rotation ownRotation;
m_robotObject->getRotation(ownRotation);
// get angles arround y-axis
double qw = ownRotation.qw();
double qy = ownRotation.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0) theta = -1.0*theta;
// rotation angle from z-axis to x-axis
double tmp = l_pos.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// direction
if(l_pos.x() > 0) targetAngle = -1.0*targetAngle;
targetAngle += theta;
double angVelFac = 3.0;
double l_angvel = m_angularVelocity/angVelFac;
if(targetAngle == 0.0) {
return 0.0;
}
else {
// circumferential distance for the rotation
double l_distance = m_distance*M_PI*fabs(targetAngle)/(2.0*M_PI);
// Duration of rotation motion (micro second)
double l_time = l_distance / (m_movingSpeed/angVelFac);
// Start the rotation
if(targetAngle > 0.0) {
m_robotObject->setWheelVelocity(l_angvel, -l_angvel);
}
else{
m_robotObject->setWheelVelocity(-l_angvel, l_angvel);
}
return l_time;
}
}
void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle)
{
if (dynamics()) { return; }
Rotation r;
r.setAxisAndAngle(ax, ay, az, angle);
const dReal *q = r.q();
setQ(q);
}
AbstractObject& Obj::rotate(double ax, double ay, double az)
{
debug("EXPR ROTATE!!!");
Rotation* t = new Rotation(ax,ay,az);
t->set_dynamic(true);
t->add(*this);
return *t;
}
Rotation AngularMotionVector3::exp() const
{
Rotation ret;
Eigen::Map<const Eigen::Vector3d> thisVec(this->data());
Eigen::AngleAxisd aa(thisVec.norm(),thisVec.normalized());
Eigen::Map< Eigen::Matrix<double,3,3,Eigen::RowMajor> >(ret.data()) = aa.toRotationMatrix();
return ret;
}
// dgoal/dq = sum( wi * ai * dai/dq ) / sum(wi)
// This calculation is modeled after Peter Eastman's gradient implementation
// in ObservedPointFitter. It treats each osensor orientation error as a
// potential energy function whose negative spatial gradient would be a spatial
// force F. We can then use Simbody's spatial force-to-generalized force method
// (using -F instead of F) to obtain the gradient in internal coordinates.
int OrientationSensors::
calcGoalGradient(const State& state, Vector& gradient) const {
const int np = getNumFreeQs();
assert(gradient.size() == np);
const SimbodyMatterSubsystem& matter = getMatterSubsystem();
Vector_<SpatialVec> dEdR(matter.getNumBodies());
dEdR = SpatialVec(Vec3(0), Vec3(0));
// Loop over each body that has one or more active osensors.
Real wtot = 0;
PerBodyOSensors::const_iterator bodyp = bodiesWithOSensors.begin();
for (; bodyp != bodiesWithOSensors.end(); ++bodyp) {
const MobilizedBodyIndex mobodIx = bodyp->first;
const Array_<OSensorIx>& bodyOSensors = bodyp->second;
const MobilizedBody& mobod = matter.getMobilizedBody(mobodIx);
const Rotation& R_GB = mobod.getBodyRotation(state);
assert(bodyOSensors.size());
// Loop over each osensor on this body.
for (unsigned m=0; m < bodyOSensors.size(); ++m) {
const OSensorIx mx = bodyOSensors[m];
const OSensor& osensor = osensors[mx];
assert(osensor.bodyB == mobodIx); // better be on this body!
const Rotation& R_GO = observations[getObservationIxForOSensor(mx)];
if (R_GO.isFinite()) { // skip NaNs
const Rotation R_GS = R_GB * osensor.orientationInB;
const Rotation R_SO = ~R_GS*R_GO; // error, in S
const Vec4 aa_SO = R_SO.convertRotationToAngleAxis();
const Vec3 trq_S = -osensor.weight * aa_SO[0]
* aa_SO.getSubVec<3>(1);
const Vec3 trq_G = R_GS * trq_S;
mobod.applyBodyTorque(state, trq_G, dEdR);
wtot += osensor.weight;
}
}
}
// Convert spatial forces dEdR to generalized forces dEdU.
Vector dEdU;
matter.multiplyBySystemJacobianTranspose(state, dEdR, dEdU);
dEdU /= wtot;
const int nq = state.getNQ();
if (np == nq) // gradient is full length
matter.multiplyByNInv(state, true, dEdU, gradient);
else { // calculate full gradient; extract the relevant parts
Vector fullGradient(nq);
matter.multiplyByNInv(state, true, dEdU, fullGradient);
for (Assembler::FreeQIndex fx(0); fx < np; ++fx)
gradient[fx] = fullGradient[getQIndexOfFreeQ(fx)];
}
return 0;
}
void SpriteRender::set_to_face_player(Player* player) {
// set this sprite's angle to point toward player
fvec3 dir = pos - player->get_viewpoint();
dir = glm::normalize(dir);
Rotation* lookdir = player->get_viewpoint_angle();
fvec3 up = glm::cross(lookdir->get_right(), dir);
up = glm::normalize(up);
Rotation pointing = Rotation();
pointing.set_to_point(dir, up);
set_angle(pointing);
}
double RobotController::getRoll(Rotation rot)
{
// get angles arround y-axis
double qw = rot.qw();
double qx = rot.qx();
double qy = rot.qy();
double qz = rot.qz();
double roll = atan2(2*qy*qw - 2*qx*qz, 1 - 2*qy*qy - 2*qz*qz);
return roll;
}
//! old task angle position controller
void ChainTask::doControl()
{
Rotation desired = Rotation::RPY(desired_values[3],desired_values[4],desired_values[5]);
Rotation measured = Rotation::RPY(chi_f_spatula(0),chi_f_spatula(1),chi_f_spatula(2));
Vector rot = diff(desired,measured);
rot = measured.Inverse()*rot;
for(unsigned int i=0;i<NC;i++) {
if(i<N_CHIF_BAKER || new_rotation)
ydot(i)=feedback_gain[i]*(desired_values[i] - chi_f(i));
else
ydot(i)=feedback_gain[i]*rot(i-N_CHIF_BAKER);
}
}
Quaternion::Quaternion(const Rotation &rotation)
: w(1), x(0), y(0), z(0)
{
/* Normalize rotation axis. */
Vector v = rotation.getAxis();
v.normalize();
/* Calculate components from given rotation axis and angle. */
float angle = rotation.getAngle();
float sinHalfAngle = std::sin(angle / 2);
w = std::cos(angle / 2);
x = v.getX() * sinHalfAngle;
y = v.getY() * sinHalfAngle;
z = v.getZ() * sinHalfAngle;
}
/*!
* @brief It rotates for the specification of the relative angle.
* @param[in] x-axis rotation weather(i of quaternion complex part)
* @param[in] y-axis rotation weather(j of quaternion complex part)
* @param[in] z-axis rotation weather(k of quaternion complex part)
* @param[in] flag for ansolute / relational (1.0=absolute, else=relational)
*/
void SimObjBase::setAxisAndAngle(double ax, double ay, double az, double angle, double direct)
{
// The angle is used now at the relative angle specification.
if (dynamics()) { return; }
Rotation r;
if (direct != 1.0) r.setQuaternion(qw(), qx(), qy(), qz());
// alculate relational angle
r.setAxisAndAngle(ax, ay, az, angle, direct);
const dReal *q = r.q();
setQ(q);
}
void generation2(StarCatalog cat, const char * fname2, Triangle central, double avg, clock_t mainStart){
clock_t start = clock() / (double)CLOCKS_PER_SEC * 1000; //###
clock_t end = clock() / (double)CLOCKS_PER_SEC * 1000; //###
cout << "Creating triangle vector" << endl;
TriangleCatalog tcat;
tcat.createCatalog(cat, avg);
end = clock() / (double)CLOCKS_PER_SEC * 1000; //###
cout << "\t#t#Trianglevector completed after " << end-start << "ms" << endl;
cout << "__________________________________________________________________________" << endl;
cout << "Vectorsearching" << endl;
Vector3D solutionAngles(0,0,0);
RID3 solutionIDs(0,0,0);
double thres = 0; //
int matchID = 0; //
clock_t vstart = clock() / (double)CLOCKS_PER_SEC * 1000; //###
while(!tcat.containsTriangle(central, thres, &solutionAngles, &solutionIDs, &matchID) && thres < 0.01) thres += 0.000001; //iteratives vergroessern des Suchthresholds
clock_t vend = clock() / (double)CLOCKS_PER_SEC * 1000; //###
TriangleEntry tmatch = tcat.at(matchID);
if(thres >= 0.01) cout << "--search inconclusive after " << vend-vstart << "ms" << endl;
else{
cout << "\t#t#Vectorsearch was conclusive after " << vend-vstart << "ms" << endl;
cout << "-Match accuracy: " << thres*180/M_PI << "°(deg) = " << thres*180/M_PI*3600 << "\"(arcsec)" << " = " << thres << " rad = " << thres/avg << " pixel-units" << endl;
cout << "-Alphas: " << solutionAngles.c[0] << " " << solutionAngles.c[1] << " " << solutionAngles.c[2] << endl;
cout << "-Beta: " << acos(-0.5*(tmatch.getD(0)*tmatch.getD(0) - tmatch.getD(1)*tmatch.getD(1) - tmatch.getD(2)*tmatch.getD(2))/(tmatch.getD(1)*tmatch.getD(2))) << endl;
cout << "-Solution IDs: " << cat.getMainCatalog().at(solutionIDs.rID[0]).getID() << " " << cat.getMainCatalog().at(solutionIDs.rID[1]).getID() << " " << cat.getMainCatalog().at(solutionIDs.rID[2]).getID() << endl;
cout << "__________________________________________________________________________" << endl;
double mainstar[3];
mainstar[0]=cat.getMainCatalog().at(solutionIDs.rID[0]).getE(0);
mainstar[1]=cat.getMainCatalog().at(solutionIDs.rID[0]).getE(1);
mainstar[2]=cat.getMainCatalog().at(solutionIDs.rID[0]).getE(2);
//cout<<mainstar[0]<<" "<<mainstar[1]<<" "<<mainstar[2]<<endl;
double nextstar[3];
nextstar[0]=cat.getMainCatalog().at(solutionIDs.rID[1]).getE(0);
nextstar[1]=cat.getMainCatalog().at(solutionIDs.rID[1]).getE(1);
nextstar[2]=cat.getMainCatalog().at(solutionIDs.rID[1]).getE(2);
//cout<<nextstar[0]<<" "<<nextstar[1]<<" "<<nextstar[2]<<endl;
Rotation r;
r.getRotationmatrix(mainstar,nextstar);
r.printRotor(1);
quaternion quat= r.getQuaternion(mainstar,nextstar);
r.printQuat(quat,1);
cout << "__________________________________________________________________________" << endl;
}
}
void EllipsoidJoint::extendSetPropertiesFromState(const SimTK::State& state)
{
Super::extendSetPropertiesFromState(state);
// Override default in case of quaternions.
const SimbodyMatterSubsystem& matter = getModel().getMatterSubsystem();
if (!matter.getUseEulerAngles(state)) {
Rotation r = getChildFrame().getMobilizedBody().getBodyRotation(state);
Vec3 angles = r.convertRotationToBodyFixedXYZ();
updCoordinate(EllipsoidJoint::Coord::Rotation1X).setDefaultValue(angles[0]);
updCoordinate(EllipsoidJoint::Coord::Rotation2Y).setDefaultValue(angles[1]);
updCoordinate(EllipsoidJoint::Coord::Rotation3Z).setDefaultValue(angles[2]);
}
}
void SimObjBase::setRotation(const Rotation &r)
{
if (dynamics()) { return; }
const dReal *q = r.q();
setQ(q);
}
Rotation Spectrum::getTypeMapping(bool inverted) const
{
if( inverted )
{
Rotation rot;
rot.assign( getType()->getDimCount(), DimUndefined );
for( int d = 0; d < rot.size(); d++ )
rot[mapToType( d )] = d;
return rot;
}else
{
Rotation rot( getDimCount() );
for( int d = 0; d < rot.size(); d++ )
rot[d] = mapToType( d );
return rot;
}
}
double MyController::calcHeadingAngle()
{
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if (qw * qy < 0) {
theta = -1 * theta;
}
return theta * 180.0 / PI;
}
//-------------------------------------------------------------------
// Test the tricked-out code used to rotation a symmetric dyadic
// matrix S_AA=R_AB*S_BB*R_BA.
bool testReexpressSymMat33() {
bool test = true;
const Rotation R_AB(Test::randRotation());
const SymMat33 S_BB(Test::randSymMat<3>());
const Mat33 M_BB(S_BB);
const SymMat33 S_AA = R_AB.reexpressSymMat33(S_BB);
const Mat33 M_AA = R_AB*M_BB*~R_AB;
const Mat33 MS_AA(S_AA);
test = test && (MS_AA-M_AA).norm() <= SignificantReal;
// Now put it back with an InverseRotation.
const SymMat33 isS_BB = (~R_AB).reexpressSymMat33(S_AA);
test = test && (S_BB-isS_BB).norm() <= SignificantReal;
const Rotation I; // identity
const SymMat33 S_BB_still = I.reexpressSymMat33(S_BB);
test = test && (S_BB_still-S_BB).norm() <= SignificantReal;
// Test symmetric matrix multiply (doesn't belong here).
const SymMat33 S1(Test::randSymMat<3>()), S2(Test::randSymMat<3>());
const Mat33 M1(S1), M2(S2);
const Mat33 S(S1*S2);
const Mat33 M(M1*M2);
test = test && (S-M).norm() <= SignificantReal;
const SymMat<3,Complex> SC1(Test::randComplex(),
Test::randComplex(), Test::randComplex(),
Test::randComplex(), Test::randComplex(), Test::randComplex() );
const SymMat<3,Complex> SC2(Test::randComplex(),
Test::randComplex(), Test::randComplex(),
Test::randComplex(), Test::randComplex(), Test::randComplex() );
SimTK_TEST_EQ(SC1.elt(1,0), conj(SC1.elt(0,1)));
SimTK_TEST_EQ(SC1.elt(2,0), conj(SC1.elt(0,2)));
SimTK_TEST_EQ(SC1.elt(1,2), conj(SC1.elt(2,1)));
const Mat<3,3,Complex> MC1(SC1), MC2(SC2);
const Mat<3,3,Complex> SC(SC1*SC2);
const Mat<3,3,Complex> MC(MC1*MC2);
SimTK_TEST_EQ(SC, MC);
return test;
}
void JacobianTest::TestChangeBase(){
//Create a random jacobian
Jacobian j1(5);
j1.data.setRandom();
//Create a random rotation
Rotation r;
random(r);
Jacobian j2(5);
CPPUNIT_ASSERT(changeBase(j1,r,j2));
CPPUNIT_ASSERT(j1!=j2);
Jacobian j3(4);
CPPUNIT_ASSERT(!changeBase(j1,r,j3));
j3.resize(5);
CPPUNIT_ASSERT(changeBase(j2,r.Inverse(),j3));
CPPUNIT_ASSERT_EQUAL(j1,j3);
}
