TEST(AgradFwdAsin,FvarFvarVar_2ndDeriv) {
using stan::math::fvar;
using stan::math::var;
using std::asin;
fvar<fvar<var> > x;
x.val_.val_ = 0.5;
x.val_.d_ = 2.0;
fvar<fvar<var> > a = asin(x);
AVEC p = createAVEC(x.val_.val_);
VEC g;
a.val_.d_.grad(p,g);
EXPECT_FLOAT_EQ(2.0 * 0.76980033, g[0]);
fvar<fvar<var> > y;
y.val_.val_ = 0.5;
y.d_.val_ = 2.0;
fvar<fvar<var> > b = asin(y);
AVEC q = createAVEC(y.val_.val_);
VEC r;
b.d_.val_.grad(q,r);
EXPECT_FLOAT_EQ(2.0 * 0.76980033, r[0]);
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
std::cout<<"orientation fucntion called"<<std::endl;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);
p << osc::BeginMessage("/myo/orientation")
<< MAC
<< quat.x() << quat.y() << quat.z() << quat.w() << roll << pitch << yaw << osc::EndMessage;
transmitSocket->Send(p.Data(), p.Size());
// Convert the floating point angles in radians to a scale from 0 to 20.
roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
//hacky globals
ROLL = roll;
PITCH = pitch;
YAW = yaw;
// Convert the floating point angles in radians to a scale from 0 to 20.
roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
if (CURRENTPOSE == myo::Pose::waveIn) {
WAVEINCOUNTER += 2;
}
if (CURRENTPOSE == myo::Pose::fingersSpread) {
FINGSERSPREADCOUNTER += 2;
}
if (WAVEINCOUNTER != 1) {
WAVEINCOUNTER -= 1;
}
}
TEST(AgradFwdAsin,FvarVar_1stDeriv) {
using stan::math::fvar;
using stan::math::var;
using std::asin;
fvar<var> x(0.5,0.3);
fvar<var> a = asin(x);
EXPECT_FLOAT_EQ(asin(0.5), a.val_.val());
EXPECT_FLOAT_EQ(0.3 / sqrt(1.0 - 0.5 * 0.5), a.d_.val());
AVEC y = createAVEC(x.val_);
VEC g;
a.val_.grad(y,g);
EXPECT_FLOAT_EQ(1.0 / sqrt(1.0 - 0.5 * 0.5), g[0]);
}
inline GAUSS gauss_ini(double e, double phi0, double &chi, double &rc)
{
using std::asin;
using std::cos;
using std::sin;
using std::sqrt;
using std::tan;
double sphi = 0;
double cphi = 0;
double es = 0;
GAUSS en;
es = e * e;
en.e = e;
sphi = sin(phi0);
cphi = cos(phi0);
cphi *= cphi;
rc = sqrt(1.0 - es) / (1.0 - es * sphi * sphi);
en.C = sqrt(1.0 + es * cphi * cphi / (1.0 - es));
chi = asin(sphi / en.C);
en.ratexp = 0.5 * en.C * e;
en.K = tan(0.5 * chi + detail::FORTPI)
/ (pow(tan(0.5 * phi0 + detail::FORTPI), en.C) * srat(en.e * sphi, en.ratexp));
return en;
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
// Convert the floating point angles in radians to a scale from 0 to 20.
float roll_f = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
float pitch_f = ((pitch + (float)M_PI/2.0f)/M_PI * 18);
float yaw_f = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
int roll_w = static_cast<int>(roll_f);
int pitch_w = static_cast<int>(pitch_f);
int yaw_w = static_cast<int>(yaw_f);
MyoData &data = knownMyosData[myo];
data.roll = roll_f/18.0*100.0;
data.pitch = pitch_f/18.0*100.0;
data.yaw = yaw_f/18.0*100.0;
sendOrientation(myo, data.roll, data.pitch, data.yaw);
}
void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
using std::max;
using std::min;
controlSystem = controller->getControlSystem();
systemUnlocked = controller->getSystemStatus();
if (controlSystem==2 && systemUnlocked){
roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
roll_w = static_cast<int>((roll + (float)M_PI/2.0f)/M_PI * 18);
controller->setRoll(roll_w);
} else {
pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
controller->setPitch(pitch_w);
//cout << "SPEED: " << controller->longSpeed[pitch_w] << endl;
}
if (controlSystem == 0 && systemUnlocked){
yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
controller->setYaw(yaw_w);
}
}
void complex_number_examples()
{
Complex z1{0, 1};
std::cout << std::setprecision(std::numeric_limits<typename Complex::value_type>::digits10);
std::cout << std::scientific << std::fixed;
std::cout << "Print a complex number: " << z1 << std::endl;
std::cout << "Square it : " << z1*z1 << std::endl;
std::cout << "Real part : " << z1.real() << " = " << real(z1) << std::endl;
std::cout << "Imaginary part : " << z1.imag() << " = " << imag(z1) << std::endl;
using std::abs;
std::cout << "Absolute value : " << abs(z1) << std::endl;
std::cout << "Argument : " << arg(z1) << std::endl;
std::cout << "Norm : " << norm(z1) << std::endl;
std::cout << "Complex conjugate : " << conj(z1) << std::endl;
std::cout << "Projection onto Riemann sphere: " << proj(z1) << std::endl;
typename Complex::value_type r = 1;
typename Complex::value_type theta = 0.8;
using std::polar;
std::cout << "Polar coordinates (phase = 0) : " << polar(r) << std::endl;
std::cout << "Polar coordinates (phase !=0) : " << polar(r, theta) << std::endl;
std::cout << "\nElementary special functions:\n";
using std::exp;
std::cout << "exp(z1) = " << exp(z1) << std::endl;
using std::log;
std::cout << "log(z1) = " << log(z1) << std::endl;
using std::log10;
std::cout << "log10(z1) = " << log10(z1) << std::endl;
using std::pow;
std::cout << "pow(z1, z1) = " << pow(z1, z1) << std::endl;
using std::sqrt;
std::cout << "Take its square root : " << sqrt(z1) << std::endl;
using std::sin;
std::cout << "sin(z1) = " << sin(z1) << std::endl;
using std::cos;
std::cout << "cos(z1) = " << cos(z1) << std::endl;
using std::tan;
std::cout << "tan(z1) = " << tan(z1) << std::endl;
using std::asin;
std::cout << "asin(z1) = " << asin(z1) << std::endl;
using std::acos;
std::cout << "acos(z1) = " << acos(z1) << std::endl;
using std::atan;
std::cout << "atan(z1) = " << atan(z1) << std::endl;
using std::sinh;
std::cout << "sinh(z1) = " << sinh(z1) << std::endl;
using std::cosh;
std::cout << "cosh(z1) = " << cosh(z1) << std::endl;
using std::tanh;
std::cout << "tanh(z1) = " << tanh(z1) << std::endl;
using std::asinh;
std::cout << "asinh(z1) = " << asinh(z1) << std::endl;
using std::acosh;
std::cout << "acosh(z1) = " << acosh(z1) << std::endl;
using std::atanh;
std::cout << "atanh(z1) = " << atanh(z1) << std::endl;
}
void DeviceCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
Device * device = findDevice(myo);
if ( device ) {
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
device->q = quat;//set(quat.x(), quat.y(), quat.z(), quat.w());
device->roll = roll;
device->pitch = pitch;
device->yaw = yaw;
// Convert the floating point angles in radians to a scale from 0 to 20.
device->roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
device->pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
device->yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
float gyoro_g = sqrt(device->gyro.x()*device->gyro.x() + device->gyro.y()*device->gyro.y() + device->gyro.z()*device->gyro.z());
float g = sqrt(device->accel.x()*device->accel.x() + device->accel.y()*device->accel.y() + device->accel.z()*device->accel.z());
// cout << gyoro_g << endl;
// cout << g << endl;
if ( gyoro_g <= 0.2 ) device->gravity = g;
// Quaternion4f q;
// q = quat;
// //.set(quat.x(), quat.z(), quat.y(), quat.w());
// Vector3f linear_accel;
// ofMatrix4x4 mat;
// mat.translate(ofVec3f(0,device->gravity,0));
// mat.rotate(q);
// ofVec3f trans = mat.getTranslation();
//
// linear_accel = device->getAccel();
// linear_accel.x = linear_accel.x - trans.x;
// linear_accel.y = linear_accel.y - trans.z;
// linear_accel.z = linear_accel.z - trans.y;
//
// device->linear_accel.set(linear_accel);
// cout << device->getAccel() << endl;
// cout << mat.getTranslation() << endl;
// cout << linear_accel << endl;
}
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
// Convert the floating point angles in radians to a scale from 0 to 20.
roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
float drop = 1.1;
roll = ((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
pitch = ((pitch + (float)M_PI/2.0f)/M_PI * 18);
if(pitch > 10)
{ if(pitch < 20)
{ pitch *= (drop*(20-pitch));
}
}
else
{ if(pitch != 10)
{ pitch *= (drop*(10-pitch));
}
}
yaw = ((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
if(yaw > 10.0000)
{ if(yaw < 20.0000)
{ yaw *= (drop*(20-yaw));
}
else
{ yaw = 19.9999;
}
}
else
{ if(yaw != 10.0000)
{ yaw *= (drop*(10-yaw));
}
}
if(color_input == true)
{ float x = (GetSystemMetrics(SM_CXSCREEN));
float y = (GetSystemMetrics(SM_CYSCREEN));
SetCursorPos((((((20)-yaw))/(20))*x), ((pitch/(20))*y));
}
}
TEST(AgradFwdAsin,FvarFvarVar_1stDeriv) {
using stan::math::fvar;
using stan::math::var;
using std::asin;
fvar<fvar<var> > x;
x.val_.val_ = 0.5;
x.val_.d_ = 2.0;
fvar<fvar<var> > a = asin(x);
EXPECT_FLOAT_EQ(asin(0.5), a.val_.val_.val());
EXPECT_FLOAT_EQ(2.0 / sqrt(1.0 - 0.5 * 0.5), a.val_.d_.val());
EXPECT_FLOAT_EQ(0, a.d_.val_.val());
EXPECT_FLOAT_EQ(0, a.d_.d_.val());
AVEC p = createAVEC(x.val_.val_);
VEC g;
a.val_.val_.grad(p,g);
EXPECT_FLOAT_EQ(1.0 / sqrt(1.0 - 0.5 * 0.5), g[0]);
fvar<fvar<var> > y;
y.val_.val_ = 0.5;
y.d_.val_ = 2.0;
fvar<fvar<var> > b = asin(y);
EXPECT_FLOAT_EQ(asin(0.5), b.val_.val_.val());
EXPECT_FLOAT_EQ(0, b.val_.d_.val());
EXPECT_FLOAT_EQ(2.0 / sqrt(1.0 - 0.5 * 0.5), b.d_.val_.val());
EXPECT_FLOAT_EQ(0, b.d_.d_.val());
AVEC q = createAVEC(y.val_.val_);
VEC r;
b.val_.val_.grad(q,r);
EXPECT_FLOAT_EQ(1.0 / sqrt(1.0 - 0.5 * 0.5), r[0]);
}
void Rotation::inverseTransform(double& lat, double& lon) const {
const double u = toRadians(lon);
const double v = toRadians(lat);
const double w = cos(v);
const double x = cos(u) * w;
const double y = sin(u) * w;
const double z = sin(v);
const double x2 = a11 * x + a21 * y + a31 * z;
const double y2 = a12 * x + a22 * y + a32 * z;
const double z2 = a13 * x + a23 * y + a33 * z;
lat = toDegrees(asin(z2));
lon = toDegrees(atan2(y2, x2));
}
TEST(AgradFwdAsin,FvarFvarVar_3rdDeriv) {
using stan::math::fvar;
using stan::math::var;
using std::asin;
fvar<fvar<var> > x;
x.val_.val_ = 0.5;
x.val_.d_ = 1.0;
x.d_.val_ = 1.0;
fvar<fvar<var> > a = asin(x);
AVEC p = createAVEC(x.val_.val_);
VEC g;
a.d_.d_.grad(p,g);
EXPECT_FLOAT_EQ(3.07920143567800, g[0]);
}
/*
* Compute the central angle, in radians, between two lat/lon pairs, in
* radians. Uses the haversine formula with a spherical geoid approximation.
*/
double
central_angle(const double lat_a, const double lon_a, const double lat_b,
const double lon_b)
{
using std::cos;
using std::asin;
using std::sqrt;
const double delta_lat = lat_b - lat_a;
const double delta_lon = lon_b - lon_a;
const double h = haversin(delta_lat) +
cos(lat_a) * cos(lat_b) * haversin(delta_lon);
const double c = 2.0 * asin( sqrt(h) );
return c;
}
void DataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
// Convert the floating point angles in radians to a scale from 0 to 20.
roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 100);
pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 100);
yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 100);
}
int vectester (Vector zerovec)
{
using std::abs;
using std::acos;
using std::asin;
using std::atan;
using std::floor;
using std::pow;
using std::sin;
using std::sqrt;
using std::tan;
typedef typename Vector::value_type Scalar;
Vector random_vec = zerovec;
Vector error_vec = zerovec;
std::srand(12345); // Fixed seed for reproduceability of failures
// Avoid divide by zero errors or acos(x>1) NaNs later
for (unsigned int i=0; i != random_vec.size(); ++i)
random_vec.raw_at(i) = .25 + (static_cast<Scalar>(std::rand())/RAND_MAX/2);
int returnval = 0;
one_test(2*random_vec - random_vec - random_vec);
one_test(3*random_vec - random_vec*3);
one_test((random_vec + random_vec)/2 - random_vec);
one_test(sqrt(random_vec) * sqrt(random_vec) - random_vec);
one_test(random_vec*random_vec - pow(random_vec,2));
one_test(sqrt(random_vec) - pow(random_vec,Scalar(.5)));
one_test(random_vec - sin(asin(random_vec)));
one_test(random_vec - tan(atan(random_vec)));
one_test(floor(random_vec / 2));
one_test(abs(random_vec) - random_vec);
return returnval;
}
void myouse::MyouseListener::onOrientationData(myo::Myo * myo, uint64_t timestamp,
const myo::Quaternion<float> & rotation)
{
using std::atan2;
using std::asin;
using std::sin;
double newRoll = atan2(2.0f * (rotation.w() * rotation.x() + rotation.y() * rotation.z()),
1.0f - 2.0f * (rotation.x() * rotation.x() + rotation.y() * rotation.y()));
double newPitch = asin(2.0f * (rotation.w() * rotation.y() - rotation.z() * rotation.x()));
double newYaw = atan2(2.0f * (rotation.w() * rotation.z() + rotation.x() * rotation.y()),
1.0f - 2.0f * (rotation.y() * rotation.y() + rotation.z() * rotation.z()));
double roll = newRoll - rollOffset;
double pitch = newPitch - pitchOffset;
double yaw = newYaw - yawOffset;
if (xDir == myo::xDirectionTowardElbow) pitch *= -1;
if (isScrolling)
{
scroll(-pitch * SCROLL_SPEED);
}
else
{
int x = SCREEN_WIDTH * (0.5 - X_SPEED * yaw);
int y = SCREEN_HEIGHT * (0.5 + Y_SPEED * pitch);
bool dragging = leftDown || rightDown || middleDown;
float dist = sqrt((x - lastX) * (x - lastX) + (y - lastY) * (y - lastY));
if (!dragging || dist > DRAG_THRESHOLD)
{
moveMouse(x, y);
lastX = x;
lastY = y;
}
}
rawRoll = newRoll;
rawPitch = newPitch;
rawYaw = newYaw;
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void onOrientationData(Myo* myo, uint64_t timestamp, const Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
using std::max;
using std::min;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
// Convert the floating point angles in radians to a scale from 0 to 18.
// roll_w = static_cast<int>((roll + (float)M_PI)/(M_PI * 2.0f) * 18);
// pitch_w = static_cast<int>((pitch + (float)M_PI/2.0f)/M_PI * 18);
// yaw_w = static_cast<int>((yaw + (float)M_PI)/(M_PI * 2.0f) * 18);
}
// onOrientationData() is called whenever the Myo device provides its current orientation, which is represented
// as a unit quaternion.
void Filter::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
quatX = quat.x(); //Return the x-component of this quaternion's vector
quatY = quat.y(); //Return the y -component of this quaternion's vector
quatZ = quat.z(); //Return the z-component of this quaternion's vector
quatW = quat.w(); //Return the w-component (scalar) of this quaternion's vector
using std::atan2;
using std::asin;
using std::sqrt;
using std::cos;
using std::sin;
using std::max;
using std::min;
roll = -1 * atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
pitch = -1 * atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
yaw = -1 * asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
}
void MyoDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat)
{
using std::atan2;
using std::asin;
using std::sqrt;
// Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(2.0f * (quat.w() * quat.y() - quat.z() * quat.x()));
float yaw = atan2(2.0f * (quat.w() * quat.z() + quat.x() * quat.y()),
1.0f - 2.0f * (quat.y() * quat.y() + quat.z() * quat.z()));
MyoFrameOrientationData & orient_data = m_orientation_data;
orient_data.timestamp = timestamp;
orient_data.quaternion = ofQuaternion( quat.x(), quat.y(), quat.z(), quat.w() );
orient_data.euler_roll = roll;
orient_data.euler_pitch = pitch;
orient_data.euler_yaw = yaw;
}
inline fvar<T> asin(const fvar<T>& x) {
using std::asin;
using std::sqrt;
return fvar<T>(asin(x.val_), x.d_ / sqrt(1 - square(x.val_)));
}
int vectester (Vector zerovec)
{
using std::abs;
using std::acos;
using std::asin;
using std::atan;
using std::ceil;
using std::cos;
using std::cosh;
using std::exp;
using std::fabs;
using std::floor;
using std::log;
using std::log10;
using std::pow;
using std::sin;
using std::sinh;
using std::sqrt;
using std::tan;
using std::tanh;
typedef typename ValueType<Vector>::type DualScalar;
typedef typename DualScalar::value_type Scalar;
Vector random_vec = zerovec;
typename DerivativeType<Vector>::type error_vec = 0;
std::srand(12345); // Fixed seed for reproduceability of failures
// Avoid divide by zero errors later
for (unsigned int i=0; i != N; ++i)
{
random_vec.raw_at(i) = .25 + (static_cast<Scalar>(std::rand())/RAND_MAX);
random_vec.raw_at(i).derivatives() = 1;
}
// Scalar pi = acos(Scalar(-1));
int returnval = 0;
// Running non-derivatives tests with DualNumbers sometimes catches
// problems too
one_test(2*random_vec - random_vec - random_vec);
one_test(3*random_vec - random_vec*3);
one_test((random_vec + random_vec)/2 - random_vec);
// We had a problem in user code with the mixing of long double and
// DualNumber<double, DynamicSparseNumberArray>
one_test(2.L*random_vec - random_vec - 1.f*random_vec);
// pow() is still having problems with sparse vectors? Disabling it
// for now.
one_test(sqrt(random_vec) * sqrt(random_vec) - random_vec);
// one_test(random_vec*random_vec - pow(random_vec,2));
// one_test(sqrt(random_vec) - pow(random_vec,Scalar(.5)));
// functions which map zero to non-zero are not defined for sparse
// vectors. This includes exp(), log(), cos(), cosh(),
// pow(scalar,sparse), scalar/sparse, sparse +/- scalar...
// one_test(log(exp(random_vec)) - random_vec);
// one_test(exp(log(random_vec)) - random_vec);
// one_test(exp(random_vec) - pow(exp(Scalar(1)), random_vec));
// one_test(tan(random_vec) - sin(random_vec)/cos(random_vec));
one_test(random_vec - sin(asin(random_vec)));
// one_test(random_vec - cos(acos(random_vec)));
one_test(random_vec - tan(atan(random_vec)));
// one_test(1 - pow(sin(random_vec), 2) - pow(cos(random_vec), 2));
// one_test(cos(random_vec) - sin(random_vec + pi/2));
// one_test(tanh(random_vec) - sinh(random_vec)/cosh(random_vec));
// one_test(1 + pow(sinh(random_vec), 2) - pow(cosh(random_vec), 2));
// one_test(log10(random_vec) - log(random_vec)/log(Scalar(10)));
one_test(floor(random_vec / 2));
// one_test(ceil(random_vec / 2 - 1));
one_test(abs(random_vec) - random_vec);
// one_test(fabs(random_vec-.75) - abs(random_vec-.75));
// And now for derivatives tests
// one_test(derivatives(pow(sin(random_vec-2),2)) -
// 2*sin(random_vec)*cos(random_vec));
// one_test(derivatives(cos(2*random_vec)) + 2*sin(2*random_vec));
// one_test(derivatives(tan(.5*random_vec)) - .5/pow(cos(.5*random_vec),2));
// one_test(derivatives(sqrt(random_vec+1)) - 1/sqrt(random_vec+1)/2);
// one_test(derivatives((random_vec-1)*(random_vec-1)) - 2*(random_vec-1));
// one_test(derivatives(pow(random_vec,1.5)) -
// 1.5*pow(random_vec,.5));
// one_test(derivatives(exp(pow(random_vec,3))) -
// exp(pow(random_vec,3))*3*pow(random_vec,2));
// one_test(derivatives(exp(random_vec)) -
// exp(random_vec));
// one_test(derivatives(pow(2,random_vec)) -
// pow(2,random_vec)*log(Scalar(2)));
// one_test(derivatives(asin(random_vec)) -
// 1/sqrt(1-random_vec*random_vec));
// one_test(derivatives(sinh(random_vec)) - cosh(random_vec));
// one_test(derivatives(cosh(random_vec)) - sinh(random_vec));
// one_test(derivatives(tanh(random_vec)) -
// derivatives(sinh(random_vec)/cosh(random_vec)));
return returnval;
}
TEST(AgradFvar, asin) {
using stan::agrad::fvar;
using std::asin;
using std::isnan;
using std::sqrt;
using stan::math::INFTY;
fvar<double> x(0.5);
x.d_ = 1.0; // derivatives w.r.t. x
fvar<double> a = asin(x);
EXPECT_FLOAT_EQ(asin(0.5), a.val_);
EXPECT_FLOAT_EQ(1 / sqrt(1 - 0.5 * 0.5), a.d_);
fvar<double> b = 2 * asin(x) + 4;
EXPECT_FLOAT_EQ(2 * asin(0.5) + 4, b.val_);
EXPECT_FLOAT_EQ(2 / sqrt(1 - 0.5 * 0.5), b.d_);
fvar<double> c = -asin(x) + 5;
EXPECT_FLOAT_EQ(-asin(0.5) + 5, c.val_);
EXPECT_FLOAT_EQ(-1 / sqrt(1 - 0.5 * 0.5), c.d_);
fvar<double> d = -3 * asin(x) + 5 * x;
EXPECT_FLOAT_EQ(-3 * asin(0.5) + 5 * 0.5, d.val_);
EXPECT_FLOAT_EQ(-3 / sqrt(1 - 0.5 * 0.5) + 5, d.d_);
fvar<double> y(3.4);
y.d_ = 1.0;
fvar<double> e = asin(y);
isnan(e.val_);
isnan(e.d_);
fvar<double> z(1.0);
z.d_ = 1.0;
fvar<double> f = asin(z);
EXPECT_FLOAT_EQ(asin(1.0), f.val_);
EXPECT_FLOAT_EQ(INFTY, f.d_);
}
void WifiDataCollector::onOrientationData(myo::Myo* myo, uint64_t timestamp, const myo::Quaternion<float>& quat) {
using std::atan2;
using std::asin;
using std::sqrt;
using std::max;
using std::min;
ClientInfo* clientinfo = new ClientInfo();
clientinfo->ID = 0;
clientinfo->message = new std::string("message");
clientinfo->name = "Client";
// Calculate Euler angles (roll and pitch) from the unit quaternion.
float roll = atan2(2.0f * (quat.w() * quat.x() + quat.y() * quat.z()),
1.0f - 2.0f * (quat.x() * quat.x() + quat.y() * quat.y()));
float pitch = asin(max(-1.0f, min(1.0f, 2.0f * (quat.w() * quat.y() - quat.z() * quat.x()))));
//Transforms the radians to degrees angles.
roll_w = (int)(roll * 180 / M_PI);
pitch_w = (int)(pitch * 180 / M_PI);
//To use in the robot servo motor, roll_w mut be between 45 and 135 degrees
//Gives an interval of 30 degrees to stabilize at 0 degrees.
//The 0 degree on the servo motor is at 90 degrees.
if (roll_w >= -15 && roll_w <= 15) {
roll_w = 90;
}
else if (roll_w < -15) {
//roll_w min value is 45.
if (roll_w < -60)
roll_w = 45;
else
roll_w += 105;
}
else if (roll_w > 15) {
//roll_w max value is 135
if (roll_w > 60)
roll_w = 135;
else
roll_w += 75;
}
//The robot has 6 diffirent speeds
//Gives an interval of 30 degrees to stabilize at 0 degrees.
// Parking state
if (pitch_w >= -15 && pitch_w <= 15) {
pitch_w = 0;
}
//Reverse speeds
else if (pitch_w < -15) {
if (pitch_w >= -35) pitch_w = 4;
else if (pitch_w >= -55) pitch_w = 5;
else pitch_w = 6;
}
//Foward speeds
else if (pitch_w > 15) {
if (pitch_w <= 35) pitch_w = 1;
else if (pitch_w <= 55) pitch_w = 2;
else pitch_w = 3;
}
clientinfo->servo = roll_w;
clientinfo->speed = pitch_w;
client->sendToServer(clientinfo);
delete clientinfo;
}
inline T0
operator()(const T0& arg1) const {
return asin(arg1);
}