// Decrements current elliptic curve point by a given point.
void epoint::operator-= (const epoint &q)
{
int r;
epoint res(*curve), qinv(*curve);
eccOperations::inv(q, qinv);
if (r = eccOperations::sum(*this, qinv, res)) eccRoutines::op_err(r);
*this = res;
}
// Returns difference of two elliptic curve points.
epoint epoint::operator- (const epoint &q) const
{
int r;
epoint res(*curve), qinv(*curve);
eccOperations::inv(q, qinv);
if (r = eccOperations::sum(*this, qinv, res)) eccRoutines::op_err(r);
return res;
}
/*
* Invert a complex number.
*/
COMPLEX *
cinv(COMPLEX *c)
{
COMPLEX *r;
NUMBER *q1, *q2, *den;
if (ciszero(c)) {
math_error("Inverting zero");
/*NOTREACHED*/
}
r = comalloc();
if (cisreal(c)) {
qfree(r->real);
r->real = qinv(c->real);
return r;
}
if (cisimag(c)) {
q1 = qinv(c->imag);
qfree(r->imag);
r->imag = qneg(q1);
qfree(q1);
return r;
}
q1 = qsquare(c->real);
q2 = qsquare(c->imag);
den = qqadd(q1, q2);
qfree(q1);
qfree(q2);
qfree(r->real);
r->real = qqdiv(c->real, den);
q1 = qqdiv(c->imag, den);
qfree(r->imag);
r->imag = qneg(q1);
qfree(q1);
qfree(den);
return r;
}
void qball(Rectangle r, Mouse *m, Quaternion *result, void (*redraw)(void), Quaternion *ap){
Quaternion q, down;
Point rad;
axis=ap;
ctlcen=divpt(addpt(r.min, r.max), 2);
rad=divpt(subpt(r.max, r.min), 2);
ctlrad=(rad.x<rad.y?rad.x:rad.y)-BORDER;
down=qinv(mouseq(m->xy));
q=*result;
for(;;){
*m=emouse();
if(!m->buttons) break;
*result=qmul(q, qmul(down, mouseq(m->xy)));
(*redraw)();
}
}
vec3 quaternion::rotate(float delta, vec3 axis, vec3 vector)
{
quaternion v(0.0f, vector);
quaternion result_quaternion;
vec3 result_vector;
float cosval = (float)fcos(delta / 2);
float sinval = (float)fsin(delta / 2);
s = cosval;
x = sinval * axis.x;
y = sinval * axis.y;
z = sinval * axis.z;
quaternion qinv(s, -x, -y, -z);
result_quaternion = (*this * v * qinv);
result_vector.x = result_quaternion.x;
result_vector.y = result_quaternion.y;
result_vector.z = result_quaternion.z;
return result_vector;
}
double qtukey(double p, double rr, double cc, double df,
int lower_tail, int log_p)
{
const static double eps = 0.0001;
const int maxiter = 50;
double ans = 0.0, valx0, valx1, x0, x1, xabs;
int iter;
#ifdef IEEE_754
if (ISNAN(p) || ISNAN(rr) || ISNAN(cc) || ISNAN(df)) {
ML_ERROR(ME_DOMAIN, "qtukey");
return p + rr + cc + df;
}
#endif
/* df must be > 1 ; there must be at least two values */
if (df < 2 || rr < 1 || cc < 2) ML_ERR_return_NAN;
R_Q_P01_boundaries(p, 0, ML_POSINF);
p = R_DT_qIv(p); /* lower_tail,non-log "p" */
/* Initial value */
x0 = qinv(p, cc, df);
/* Find prob(value < x0) */
valx0 = ptukey(x0, rr, cc, df, /*LOWER*/TRUE, /*LOG_P*/FALSE) - p;
/* Find the second iterate and prob(value < x1). */
/* If the first iterate has probability value */
/* exceeding p then second iterate is 1 less than */
/* first iterate; otherwise it is 1 greater. */
if (valx0 > 0.0)
x1 = fmax2(0.0, x0 - 1.0);
else
x1 = x0 + 1.0;
valx1 = ptukey(x1, rr, cc, df, /*LOWER*/TRUE, /*LOG_P*/FALSE) - p;
/* Find new iterate */
for(iter=1 ; iter < maxiter ; iter++) {
ans = x1 - ((valx1 * (x1 - x0)) / (valx1 - valx0));
valx0 = valx1;
/* New iterate must be >= 0 */
x0 = x1;
if (ans < 0.0) {
ans = 0.0;
valx1 = -p;
}
/* Find prob(value < new iterate) */
valx1 = ptukey(ans, rr, cc, df, /*LOWER*/TRUE, /*LOG_P*/FALSE) - p;
x1 = ans;
/* If the difference between two successive */
/* iterates is less than eps, stop */
xabs = fabs(x1 - x0);
if (xabs < eps)
return ans;
}
/* The process did not converge in 'maxiter' iterations */
ML_ERROR(ME_NOCONV, "qtukey");
return ans;
}
#include "qmath.h"
#include "brad_pitch_instrumental.h"
#include "definebrad.h"
#include "brad_input.h"
void process_pitchshift(PShift *this, fixedp *process) {
int i;
fixedp in, out, next;
fixedp rp2, frac, fi;
fixedp wetSignal;
int rpi, ep1, ep2;
fixedp fixedBufSize = short2q(PSHIFT_BUFSIZE);
fixedp bufOverTwo = 2 * qinv( fixedBufSize );
for(i = 0; i < PROCESS_SIZE; i++ ) {
in = process[i];
fi = int2q(i);
// Envelope pointer1
ep1 = q2int( qsub(this->rp1, fi));
if(ep1 < 0) ep1 = qadd(ep1, PSHIFT_BUFSIZE);
// Calculate read pointer 2 and envelope pointer 2
rp2 = qadd(this->rp1, bufOverTwo);
if(rp2 >= fixedBufSize) rp2 = qsub(rp2, fixedBufSize);
ep2 = q2int( qsub( rp2, fi ) );
if(ep2 < 0) ep2 = qadd(ep2, PSHIFT_BUFSIZE);
// Get signal for read pointer 1
int
main(int argc, char** argv)
{
if (argc < 2)
{
std::cout << "Usage: rlInverseKinematicsTest KINEMATICSFILE" << std::endl;
return 1;
}
try
{
std::srand(0); // get reproducible results
rl::kin::Kinematics* kinematics = static_cast< ::rl::kin::Kinematics* >(rl::kin::Kinematics::create(argv[1]));
std::size_t nTests;
rl::math::Vector q(6);
rl::math::Vector qinv(6);
rl::math::Vector qzero(6);
std::size_t n;
std::size_t wrongs;
std::size_t wrongT;
std::size_t ngotinverse;
for (std::size_t ispuma = 0; ispuma < 2; ++ispuma)
{
nTests = 0 == ispuma ? 1000 : 100;
for (n = 0, wrongs = 0, wrongT = 0, ngotinverse = 0; n < nTests && wrongT < 100 && wrongs < 100; ++n)
{
for (std::size_t i = 0; i < 6; ++i)
{
rl::math::Real r = static_cast< rl::math::Real >(std::rand()) / static_cast< rl::math::Real >(RAND_MAX);
q(i) = (r - 0.5) * 360 * rl::math::DEG2RAD;
qzero(i) = 0;
}
kinematics->setPosition(q);
kinematics->updateFrames();
rl::math::Transform t = kinematics->forwardPosition();
// For iterative inverse, set starting point far away
kinematics->setPosition(qzero);
kinematics->updateFrames();
if (0 == ispuma)
{
rl::kin::Puma::Arm arm;
rl::kin::Puma::Elbow elbow;
rl::kin::Puma::Wrist wrist;
dynamic_cast< rl::kin::Puma* >(kinematics)->parameters(q, arm, elbow, wrist);
dynamic_cast< rl::kin::Puma* >(kinematics)->setArm(arm);
dynamic_cast< rl::kin::Puma* >(kinematics)->setElbow(elbow);
dynamic_cast< rl::kin::Puma* >(kinematics)->setWrist(wrist);
}
if (
0 == ispuma
? dynamic_cast< ::rl::kin::Puma* >(kinematics)->inversePosition(t, qinv)
: dynamic_cast< ::rl::kin::Kinematics* >(kinematics)->inversePosition(t, qinv, 0, 100)
)
{
kinematics->setPosition(qinv);
kinematics->updateFrames();
rl::math::Transform tinv = kinematics->forwardPosition();
if ((t.matrix() - tinv.matrix()).norm() > 1e-6)
{
++wrongT;
}
if ((q - qinv).norm() > 1e-4)
{
++wrongs;
}
if (wrongT < 3 && (t.matrix() - tinv.matrix()).norm() > 1e-6)
{
std::cout << " q = " << q.transpose() << std::endl;
std::cout << " T = " << t.matrix() << std::endl;
std::cout << " qinv = " << qinv.transpose() << std::endl;
std::cout << "Wrong Tinv = " << tinv.matrix() << std::endl;
std::cout << std::endl;
}
++ngotinverse;
}
}
std::cout << "Notice: "
<< (0 == ispuma ? "Puma direct " : "Iterative ") << "inverse kinematics "
<< "on file " << argv[1] << " "
<< "tested with " << n << " cases, "
<< ngotinverse << " returned a solution, "
<< "thereof " << wrongs << " in wrong configuration, and "
<< wrongT << " with completely wrong pose."
<< std::endl;
if (wrongT > 0)
{
std::cerr << "Error: "
<< (0 == ispuma ? "Puma direct " : "Iterative ")
<< "inverse kinematics " << "on file " << argv[1]
<< " gave incorrect poses." << std::endl;
return 1;
}
if (0 == ngotinverse)
{
std::cerr << "Error: "
<< (0 == ispuma ? "Puma direct " : "Iterative ")
<< "inverse kinematics "<< "on file " << argv[1]
<< " gave no solutions."
<< std::endl;
return 1;
}
}
}
catch (const std::exception& e)
{
std::cerr << e.what() << std::endl;
return 1;
}
return 0;
}
