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]); } }