void spTransformConstraint_apply (spTransformConstraint* self) {
float rotateMix = self->rotateMix, translateMix = self->translateMix, scaleMix = self->scaleMix, shearMix = self->shearMix;
spBone* target = self->target;
float ta = target->a, tb = target->b, tc = target->c, td = target->d;
int i;
for (i = 0; i < self->bonesCount; ++i) {
spBone* bone = self->bones[i];
if (rotateMix > 0) {
float a = bone->a, b = bone->b, c = bone->c, d = bone->d;
float r = ATAN2(tc, ta) - ATAN2(c, a) + self->data->offsetRotation * DEG_RAD;
float cosine, sine;
if (r > PI) r -= PI2;
else if (r < -PI) r += PI2;
r *= rotateMix;
cosine = COS(r);
sine = SIN(r);
CONST_CAST(float, bone->a) = cosine * a - sine * c;
CONST_CAST(float, bone->b) = cosine * b - sine * d;
CONST_CAST(float, bone->c) = sine * a + cosine * c;
CONST_CAST(float, bone->d) = sine * b + cosine * d;
}
if (translateMix > 0) {
float x, y;
spBone_localToWorld(target, self->data->offsetX, self->data->offsetY, &x, &y);
CONST_CAST(float, bone->worldX) += (x - bone->worldX) * translateMix;
CONST_CAST(float, bone->worldY) += (y - bone->worldY) * translateMix;
}
int esweep_arg(esweep_object *obj) { /* UNTESTED */
Complex *cpx;
Polar *polar;
int i;
ESWEEP_OBJ_NOTEMPTY(obj, ERR_EMPTY_OBJECT);
switch (obj->type) {
case COMPLEX:
cpx=(Complex*) obj->data;
polar=(Polar*) cpx;
for (i=0; i<obj->size; i++) {
polar[i].arg=ATAN2(cpx[i]);
polar[i].abs=0.0;
}
break;
case POLAR:
polar=(Polar*) obj->data;
for (i=0; i<obj->size; i++) polar[i].abs=0;
break;
default:
return ERR_NOT_ON_THIS_TYPE;
}
ESWEEP_ASSERT(correctFpException(obj), ERR_FP);
return ERR_OK;
}
void adjust_speed(FLOAT *speedx, FLOAT *speedy, double adjust)
{
double angle;
angle = ATAN2(*speedy, *speedx);
*speedx += (FLOAT)(adjust * COS(angle));
*speedy += (FLOAT)(adjust * SIN(angle));
}
void spIkConstraint_apply1 (spBone* bone, float targetX, float targetY, float alpha) {
spBone* pp = bone->parent;
float id = 1 / (pp->a * pp->d - pp->b * pp->c);
float x = targetX - pp->worldX, y = targetY - pp->worldY;
float tx = (x * pp->d - y * pp->b) * id - bone->x, ty = (y * pp->a - x * pp->c) * id - bone->y;
float rotationIK = ATAN2(ty, tx) * RAD_DEG - bone->shearX - bone->rotation;
if (bone->scaleX < 0) rotationIK += 180;
if (rotationIK > 180) rotationIK -= 360;
else if (rotationIK < -180) rotationIK += 360;
spBone_updateWorldTransformWith(bone, bone->x, bone->y, bone->rotation + rotationIK * alpha, bone->scaleX,
bone->scaleY, bone->shearX, bone->shearY);
}
// update information for current-cursor position ---------------------------
void __fastcall TPlot::UpdatePoint(int x, int y)
{
gtime_t time;
TPoint p(x,y);
double enu[3]={0},rr[3],pos[3],xx,yy,r,xl[2],yl[2],q[2],az,el,snr;
int i;
char tstr[64];
AnsiString msg;
trace(4,"UpdatePoint: x=%d y=%d\n",x,y);
if (PlotType==PLOT_TRK) { // track-plot
if (norm(OPos,3)>0.0) {
GraphT->ToPos(p,enu[0],enu[1]);
ecef2pos(OPos,pos);
enu2ecef(pos,enu,rr);
for (i=0;i<3;i++) rr[i]+=OPos[i];
ecef2pos(rr,pos);
msg=LatLonStr(pos,8);
}
}
else if (PlotType==PLOT_SKY||PlotType==PLOT_MPS) { // sky-plot
GraphS->GetLim(xl,yl);
GraphS->ToPos(p,q[0],q[1]);
r=(xl[1]-xl[0]<yl[1]-yl[0]?xl[1]-xl[0]:yl[1]-yl[0])*0.45;
if ((el=90.0-90.0*norm(q,2)/r)>0.0) {
az=el>=90.0?0.0:ATAN2(q[0],q[1])*R2D;
if (az<0.0) az+=360.0;
msg.sprintf("AZ=%5.1f" CHARDEG " EL=%4.1f" CHARDEG,az,el);
}
}
else if (PlotType==PLOT_SNRE) { // snr-el-plot
GraphE[0]->ToPos(p,q[0],q[1]);
msg.sprintf("EL=%4.1f " CHARDEG,q[0]);
}
else {
GraphG[0]->ToPos(p,xx,yy);
time=gpst2time(Week,xx);
if (TimeLabel==2) time=utc2gpst(time); // UTC
else if (TimeLabel==3) time=timeadd(gpst2utc(time),-9*3600.0); // JST
TimeStr(time,0,1,tstr);
msg=tstr;
}
Panel22->Visible=true;
Message2->Caption=A2U(msg);
}
/**
* Morgan SCARA Inverse Kinematics. Results in delta[].
*
* See http://forums.reprap.org/read.php?185,283327
*
* Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny.
*/
void inverse_kinematics(const float (&raw)[XYZ]) {
static float C2, S2, SK1, SK2, THETA, PSI;
float sx = raw[X_AXIS] - SCARA_OFFSET_X, // Translate SCARA to standard X Y
sy = raw[Y_AXIS] - SCARA_OFFSET_Y; // With scaling factor.
if (L1 == L2)
C2 = HYPOT2(sx, sy) / L1_2_2 - 1;
else
C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2);
S2 = SQRT(1 - sq(C2));
// Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
SK1 = L1 + L2 * C2;
// Rotated Arm2 gives the distance from Arm1 to Arm2
SK2 = L2 * S2;
// Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
THETA = ATAN2(SK1, SK2) - ATAN2(sx, sy);
// Angle of Arm2
PSI = ATAN2(S2, C2);
delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle
delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor)
delta[C_AXIS] = raw[Z_AXIS];
/*
DEBUG_POS("SCARA IK", raw);
DEBUG_POS("SCARA IK", delta);
SERIAL_ECHOLNPAIR(" SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI);
//*/
}
void aubio_fft_get_phas(fvec_t * compspec, cvec_t * spectrum) {
uint_t i;
if (compspec->data[0] < 0) {
spectrum->phas[0] = PI;
} else {
spectrum->phas[0] = 0.;
}
for (i=1; i < spectrum->length - 1; i++) {
spectrum->phas[i] = ATAN2(compspec->data[compspec->length-i],
compspec->data[i]);
}
if (compspec->data[compspec->length/2] < 0) {
spectrum->phas[spectrum->length - 1] = PI;
} else {
spectrum->phas[spectrum->length - 1] = 0.;
}
}
int esweep_lg(esweep_object *obj) { /* logarithm to base 10 */
Wave *wave;
Polar *polar;
Surface *surface;
Complex *cpx;
int i, zsize;
Real abs, arg;
ESWEEP_OBJ_NOTEMPTY(obj, ERR_EMPTY_OBJECT);
switch (obj->type) {
case WAVE:
wave=(Wave*) obj->data;
for (i=0; i<obj->size; i++) {
wave[i]=LG(FABS(wave[i]));
}
break;
case POLAR:
polar=(Polar*) obj->data;
for (i=0; i<obj->size; i++) {
polar[i].abs=LG(polar[i].abs);
}
break;
case SURFACE:
surface=(Surface*) obj->data;
zsize=surface->xsize*(surface->ysize);
for (i=0; i<zsize; i++) surface->z[i]=LG(surface->z[i]);
break;
case COMPLEX:
cpx=(Complex*) obj->data;
for (i=0; i<obj->size; i++) {
abs=CABS(cpx[i]);
arg=ATAN2(cpx[i]);
cpx[i].real=LG(abs);
cpx[i].imag=arg;
}
break;
default:
ESWEEP_NOT_THIS_TYPE(obj->type, ERR_NOT_ON_THIS_TYPE);
}
ESWEEP_ASSERT(correctFpException(obj), ERR_FP);
return ERR_OK;
}
/* ---------------------------------------------------------------------------- */
PRIVATE void
pll(FMD fm, COMPLEX sig) {
COMPLEX z = Cmplx((REAL) cos(fm->pll.phs), (IMAG) sin(fm->pll.phs));
REAL diff;
fm->pll.delay.re = z.re * sig.re + z.im * sig.im;
fm->pll.delay.im = -z.im * sig.re + z.re * sig.im;
diff = ATAN2(fm->pll.delay.im, fm->pll.delay.re);
fm->pll.freq.f += fm->pll.beta * diff;
if (fm->pll.freq.f < fm->pll.freq.l)
fm->pll.freq.f = fm->pll.freq.l;
if (fm->pll.freq.f > fm->pll.freq.h)
fm->pll.freq.f = fm->pll.freq.h;
fm->pll.phs += fm->pll.freq.f + fm->pll.alpha * diff;
while (fm->pll.phs >= TWOPI)
fm->pll.phs -= (REAL) TWOPI;
while (fm->pll.phs < 0)
fm->pll.phs += (REAL) TWOPI;
}
single XATAN2( single *arg1, single *arg2 ) {
//===========================================
return( ATAN2( *arg1, *arg2 ) );
}
void spIkConstraint_apply2 (spBone* parent, spBone* child, float targetX, float targetY, int bendDir, float alpha) {
float px = parent->x, py = parent->y, psx = parent->scaleX, psy = parent->scaleY;
float cx = child->x, cy, csx = child->scaleX, cwx, cwy;
int o1, o2, s2, u;
spBone* pp = parent->parent;
float tx, ty, dx, dy, l1, l2, a1, a2, r;
float id, x, y;
if (alpha == 0) {
spBone_updateWorldTransform(child);
return;
}
if (psx < 0) {
psx = -psx;
o1 = 180;
s2 = -1;
} else {
o1 = 0;
s2 = 1;
}
if (psy < 0) {
psy = -psy;
s2 = -s2;
}
if (csx < 0) {
csx = -csx;
o2 = 180;
} else
o2 = 0;
r = psx - psy;
u = (r < 0 ? -r : r) <= 0.0001f;
if (!u) {
cy = 0;
cwx = parent->a * cx + parent->worldX;
cwy = parent->c * cx + parent->worldY;
} else {
cy = child->y;
cwx = parent->a * cx + parent->b * cy + parent->worldX;
cwy = parent->c * cx + parent->d * cy + parent->worldY;
}
id = 1 / (pp->a * pp->d - pp->b * pp->c);
x = targetX - pp->worldX;
y = targetY - pp->worldY;
tx = (x * pp->d - y * pp->b) * id - px;
ty = (y * pp->a - x * pp->c) * id - py;
x = cwx - pp->worldX;
y = cwy - pp->worldY;
dx = (x * pp->d - y * pp->b) * id - px;
dy = (y * pp->a - x * pp->c) * id - py;
l1 = SQRT(dx * dx + dy * dy);
l2 = child->data->length * csx;
if (u) {
float cosine, a, b;
l2 *= psx;
cosine = (tx * tx + ty * ty - l1 * l1 - l2 * l2) / (2 * l1 * l2);
if (cosine < -1) cosine = -1;
else if (cosine > 1) cosine = 1;
a2 = ACOS(cosine) * bendDir;
a = l1 + l2 * cosine;
b = l2 * SIN(a2);
a1 = ATAN2(ty * a - tx * b, tx * a + ty * b);
} else {
float a = psx * l2, b = psy * l2;
float aa = a * a, bb = b * b, ll = l1 * l1, dd = tx * tx + ty * ty, ta = ATAN2(ty, tx);
float c0 = bb * ll + aa * dd - aa * bb, c1 = -2 * bb * l1, c2 = bb - aa;
float d = c1 * c1 - 4 * c2 * c0;
float minAngle = 0, minDist = FLT_MAX, minX = 0, minY = 0;
float maxAngle = 0, maxDist = 0, maxX = 0, maxY = 0;
float x = l1 + a, dist = x * x, angle, y;
if (d >= 0) {
float q = SQRT(d), r0, r1;
if (c1 < 0) q = -q;
q = -(c1 + q) / 2;
r0 = q / c2; r1 = c0 / q;
r = ABS(r0) < ABS(r1) ? r0 : r1;
if (r * r <= dd) {
y = SQRT(dd - r * r) * bendDir;
a1 = ta - ATAN2(y, r);
a2 = ATAN2(y / psy, (r - l1) / psx);
goto outer;
}
}
if (dist > maxDist) {
maxAngle = 0;
maxDist = dist;
maxX = x;
}
x = l1 - a;
dist = x * x;
if (dist < minDist) {
minAngle = PI;
minDist = dist;
minX = x;
}
angle = ACOS(-a * l1 / (aa - bb));
x = a * COS(angle) + l1;
y = b * SIN(angle);
dist = x * x + y * y;
if (dist < minDist) {
minAngle = angle;
minDist = dist;
minX = x;
minY = y;
}
if (dist > maxDist) {
maxAngle = angle;
maxDist = dist;
maxX = x;
maxY = y;
}
if (dd <= (minDist + maxDist) / 2) {
a1 = ta - ATAN2(minY * bendDir, minX);
a2 = minAngle * bendDir;
} else {
a1 = ta - ATAN2(maxY * bendDir, maxX);
a2 = maxAngle * bendDir;
}
}
outer: {
float os = ATAN2(cy, cx) * s2;
a1 = (a1 - os) * RAD_DEG + o1 - parent->rotation;
if (a1 > 180) a1 -= 360;
else if (a1 < -180) a1 += 360;
spBone_updateWorldTransformWith(parent, px, py, parent->rotation + a1 * alpha, parent->scaleX, parent->scaleY, 0, 0);
a2 = ((a2 + os) * RAD_DEG - child->shearX) * s2 + o2 - child->rotation;
if (a2 > 180) a2 -= 360;
else if (a2 < -180) a2 += 360;
spBone_updateWorldTransformWith(child, cx, cy, child->rotation + a2 * alpha, child->scaleX, child->scaleY, child->shearX, child->shearY);
}
}
/* ---------------------------------------------------------------------------- */
COMPLEX
Cr2p(COMPLEX z) {
return Cmplx((REAL) hypot(z.re, z.im),
(REAL) ATAN2(z.im, z.re));
}
//===================================================================================
float laneHalfAngle (Pair passerLoc, // Position of the passing robot.
Pair passDestination, // location of the pass destination
const VisionData &field, // where everyone else is now
const SystemParameters &rp, // contains necessary game parameters
const Pair * extraObstacle, // Check this (optional) location as an obstacle too
bool checkOurRobots) //to see if we check for our robots or not...
{
float minLaneAngle = PI/2.0f; // Initialize the lane half angle to PI/2 (a clear lane)
// Lane parameters
float laneLength = passerLoc.distanceTo (passDestination);
laneLength = laneLength + rp.general.PLAYER_RADIUS + rp.general.BALL_RADIUS;
float laneDirection = angleWithXAxis (passerLoc, passDestination);
// Obstacle parameters
Pair obstacleLoc; // Obstacle centre location
float obstacleDist; // Distance of the obstacle centre from the passerLoc
float obstacleDirection; // Angle made by the segment from passerLoc to obstacle with the x-axis
float obstacleHalfAngle; // Half the angle subtended by the obstacle on the passer loc
float angleWithLane1; // Angle between the tangents drawn from passerLoc to obstacle with
float angleWithLane2; // the lane direction
// Check opponent robots first
RobotIndex i;
for (i = ROBOT0; i < NUM_ROBOTS; i++) {
if (theirRobotFound (i, field, rp)) {
// Get obstacle direction and distance from the passerLoc
obstacleLoc = getTheirRobotLocation (i, field, rp);
obstacleDist = passerLoc.distanceTo (obstacleLoc);
// Get the direction of the segment from passerLoc to obstacle with the x-axis
obstacleDirection = angleWithXAxis (passerLoc, obstacleLoc);
obstacleHalfAngle = ATAN2 (rp.general.OPPONENT_RADIUS, obstacleDist);
// Evaluate angles only if the passer loc is closer to the obtacle than the lane length
if (obstacleDist <= laneLength) {
// If the difference between the obstacle direction and lane direction is within the half
// angle subtended by the obstacle on the passer loc, the lane is being blocked
if (ABS (laneDirection - obstacleDirection) <= obstacleHalfAngle)
return 0.0f;
// Get the directions of the tangent to the obstacle from the passer loc and their angular
// difference from the lane direction
angleWithLane1 = ABS (angleDifference (obstacleDirection + obstacleHalfAngle, laneDirection));
angleWithLane2 = ABS (angleDifference (obstacleDirection - obstacleHalfAngle, laneDirection));
// Compare the angle of the tangent closer to the lane diretion
float smallerAngle = MIN (angleWithLane1, angleWithLane2);
if (smallerAngle < minLaneAngle)
minLaneAngle = smallerAngle;
}
}
}
//IF WE WANT TO TAKE INTO ACCOUNT OUR OWN ROBOTS...
if (checkOurRobots)
{
for (i = ROBOT0; i < NUM_ROBOTS; i++) {
//If our robot is found, and he is not the passer or receiver => check to see if it is in the way
if(robotFound(i, field, rp)&&(dist(i,passerLoc,field,rp)>rp.general.PLAYER_RADIUS)&&(dist(i,passDestination,field,rp)>rp.general.PLAYER_RADIUS)) {
// Get obstacle direction and distance from the passerLoc
obstacleLoc = getLocation (i, field, rp);
obstacleDist = passerLoc.distanceTo (obstacleLoc);
// Get the direction of the segment from passerLoc to obstacle with the x-axis
obstacleDirection = angleWithXAxis (passerLoc, obstacleLoc);
obstacleHalfAngle = ATAN2 (rp.general.PLAYER_RADIUS, obstacleDist);
// Evaluate angles only if the passer loc is closer to the obtacle than the lane length
if (obstacleDist <= laneLength) {
// If the difference between the obstacle direction and lane direction is within the half
// angle subtended by the obstacle on the passer loc, the lane is being blocked
if (ABS (laneDirection - obstacleDirection) <= obstacleHalfAngle)
return 0.0f;
// Get the directions of the tangent to the obstacle from the passer loc and their angular
// difference from the lane direction
angleWithLane1 = ABS (angleDifference (obstacleDirection + obstacleHalfAngle, laneDirection));
angleWithLane2 = ABS (angleDifference (obstacleDirection - obstacleHalfAngle, laneDirection));
// Compare the angle of the tangent closer to the lane diretion
float smallerAngle = MIN (angleWithLane1, angleWithLane2);
if (smallerAngle < minLaneAngle)
minLaneAngle = smallerAngle;
}
}
}
}
// Check the passed extra location as an obstacle having the radius the same as our robots
if (extraObstacle) {
// Get obstacle direction and distance from the passerLoc
obstacleLoc = * extraObstacle;
obstacleDist = passerLoc.distanceTo (obstacleLoc);
// Get the direction of the segment from passerLoc to obstacle with the x-axis
obstacleDirection = angleWithXAxis (passerLoc, obstacleLoc);
obstacleHalfAngle = ATAN2 (rp.general.PLAYER_RADIUS, obstacleDist);
// Evaluate angles only if the passer loc is closer to the obtacle than the lane length
if (obstacleDist <= laneLength) {
// If the difference between the obstacle direction and lane direction is within the half
// angle subtended by the obstacle on the passer loc, the lane is being blocked
if (ABS (laneDirection - obstacleDirection) <= obstacleHalfAngle)
return 0.0f;
// Get the directions of the tangent to the obstacle from the passer loc and their angular
// difference from the lane direction
angleWithLane1 = ABS (angleDifference (obstacleDirection + obstacleHalfAngle, laneDirection));
angleWithLane2 = ABS (angleDifference (obstacleDirection - obstacleHalfAngle, laneDirection));
// Compare the angle of the tangent closer to the lane diretion
float smallerAngle = MIN (angleWithLane1, angleWithLane2);
if (smallerAngle < minLaneAngle)
minLaneAngle = smallerAngle;
}
}
return minLaneAngle;
}
void KERNEL_NAME(VMLLONG n, VML_FLOAT * a, VML_FLOAT * b, VML_FLOAT * y, VML_FLOAT * z, VML_FLOAT * other_params) {
VMLLONG i=0;
for(i=0; i<n; i++){
y[i]=ATAN2(a[i], b[i]);
}
}
