/// accel_to_lean_angles - horizontal desired acceleration to lean angles /// converts desired accelerations provided in lat/lon frame to roll/pitch angles void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler) { float accel_right, accel_forward; float lean_angle_max = _attitude_control.lean_angle_max(); // reset accel to current desired acceleration if (_flags.reset_accel_to_lean_xy) { _accel_target_jerk_limited.x = _accel_target.x; _accel_target_jerk_limited.y = _accel_target.y; _accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y)); _flags.reset_accel_to_lean_xy = false; } // apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec float max_delta_accel = dt * POSCONTROL_JERK_LIMIT_CMSSS; Vector2f accel_in(_accel_target.x, _accel_target.y); Vector2f accel_change = accel_in-_accel_target_jerk_limited; float accel_change_length = accel_change.length(); if(accel_change_length > max_delta_accel) { accel_change *= max_delta_accel/accel_change_length; } _accel_target_jerk_limited += accel_change; // lowpass filter on NE accel _accel_target_filter.set_cutoff_frequency(min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler)); Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt); // rotate accelerations into body forward-right frame accel_forward = accel_target_filtered.x*_ahrs.cos_yaw() + accel_target_filtered.y*_ahrs.sin_yaw(); accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw(); // update angle targets that will be passed to stabilize controller _pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI_F),-lean_angle_max, lean_angle_max); float cos_pitch_target = cosf(_pitch_target*M_PI_F/18000); _roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI_F), -lean_angle_max, lean_angle_max); }
void Stars::render(float fovY, float aspect, float nearZ, float alpha) { // determine length of screen diagonal from quadrant height and aspect ratio float quadrantHeight = nearZ * tanf(RADIANS_PER_DEGREE * fovY * 0.5f); float halfDiagonal = sqrt(quadrantHeight * quadrantHeight * (1.0f + aspect * aspect)); // determine fov angle in respect to the diagonal float fovDiagonal = atanf(halfDiagonal / nearZ) * 2.0f; // pull the modelview matrix off the GL stack glm::mat4 view; glGetFloatv(GL_MODELVIEW_MATRIX, glm::value_ptr(view)); _controller->render(fovDiagonal, aspect, glm::affineInverse(view), alpha); }
//---------------------------------------------------------------------- // 放物線(出力、開始点、目標地点、初速、かかる負荷(重さ)) //---------------------------------------------------------------------- bool Parabola(Vector3& out, Vector3 start, Vector3 end, float speed, float mass) { float angleTan; float elevation; //仰角 float height = start.y - end.y; //高低差 float tx = end.x - start.x; // X座標差 float tz = end.z - start.z; // Z座標差 float txz = sqrtf(tx*tx + tz*tz); // y = v0 * sinθ*t + 0.5f*G*t*t // x = v0 * cosθ*t // 2次方程式 a*T*T + b*T + c = 0 float A = (mass*txz*txz) / (2.0f*speed*speed); // Tの係数(aT*T + b*T + c = 0) float a, b, c; a = 1.0f; if (A != 0){ b = txz / A; c = 1.0f + height / A; } // 解の公式(平方根) float D = b * b - 4 * a * c; if (D < 0) { out = Vector3(0, 0, 0); angleTan = 0; return false; } // 解の公式 elevation = (-b + sqrtf(D)) / (2.0f*a); // 仰角 angleTan = atanf(elevation); // 移動距離 D3DXVECTOR3 tt; tt.x = end.x - start.x; tt.y = 0; tt.z = end.z - start.z; D3DXVec3Normalize(&tt, &tt); out.x = speed * cosf(angleTan)*tt.x; out.y = speed * sinf(angleTan); out.z = speed * cosf(angleTan)*tt.z; return true; }
void setup_projection_matrix(int w, int h, float horizontal_fov_degrees, float nearz, float farz) // Set up viewport & projection matrix. // w and h are the window dimensions in pixels. // horizontal_fov is in degrees. { glViewport(0, 0, w, h); // // Set up projection matrix. // float aspect_ratio = float(h) / float(w); float horizontal_fov = (float) (horizontal_fov_degrees * M_PI / 180.f); float vertical_fov = atanf(tanf(horizontal_fov / 2.f) * aspect_ratio) * 2; glMatrixMode(GL_PROJECTION); glLoadIdentity(); float m[16]; int i; for (i = 0; i < 16; i++) m[i] = 0; m[0] = -1.0f / tanf(vertical_fov / 2.0f); m[5] = -m[0] / aspect_ratio; m[10] = (farz + nearz) / (farz - nearz); m[11] = 1; m[14] = - 2 * farz * nearz / (farz - nearz); glMultMatrixf(m); glMatrixMode(GL_MODELVIEW); //#define TEST_FRUSTUM 1 // define to shrink the frustum, to see culling at work... #if TEST_FRUSTUM // Compute values for the frustum planes. Normals point inwards towards the visible volume. frustum_plane[0].set(0, 0, 1, nearz * 10); // near. frustum_plane[1].set(0, 0, -1, -farz * 0.1); // far. frustum_plane[2].set(-cosf(horizontal_fov/2), 0, sinf(horizontal_fov/4), 0); // left. frustum_plane[3].set(cosf(horizontal_fov/2), 0, sinf(horizontal_fov/4), 0); // right. frustum_plane[4].set(0, -cosf(vertical_fov/2), sinf(vertical_fov/4), 0); // top. frustum_plane[5].set(0, cosf(vertical_fov/2), sinf(vertical_fov/4), 0); // bottom. #else // Compute values for the frustum planes. Normals point inwards towards the visible volume. frustum_plane[0].set(0, 0, 1, nearz); // near. frustum_plane[1].set(0, 0, -1, -farz); // far. frustum_plane[2].set(-cosf(horizontal_fov/2), 0, sinf(horizontal_fov/2), 0); // left. frustum_plane[3].set(cosf(horizontal_fov/2), 0, sinf(horizontal_fov/2), 0); // right. frustum_plane[4].set(0, -cosf(vertical_fov/2), sinf(vertical_fov/2), 0); // top. frustum_plane[5].set(0, cosf(vertical_fov/2), sinf(vertical_fov/2), 0); // bottom. #endif }
static int GM_CDECL gmfATan(gmThread * a_thread) { GM_CHECK_NUM_PARAMS(1); float floatValue; if(a_thread->ParamType(0) == GM_INT) { floatValue = (float) a_thread->Param(0).m_value.m_int; } else if(a_thread->ParamType(0) == GM_FLOAT) { floatValue = a_thread->Param(0).m_value.m_float; } else { return GM_EXCEPTION; } a_thread->PushFloat(atanf(floatValue)); return GM_OK; }
bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, float joint0, bool left, const RobotDimensions& robotDimensions) { Pose3D target(position); Joint firstJoint(left ? LHipYawPitch : RHipYawPitch); const int sign(left ? -1 : 1); target.translation.y += robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2 * sign; // translate to origin of leg target = Pose3D().rotateZ(joint0 * -sign).rotateX(M_PI_4 * sign).conc(target); // compute residual transformation with fixed joint0 // use cosine theorem and arctan to compute first three joints float length = target.translation.abs(); float sqrLength = length * length; float upperLeg = robotDimensions.values_[RobotDimensions::upperLegLength]; float sqrUpperLeg = upperLeg * upperLeg; float lowerLeg = robotDimensions.values_[RobotDimensions::lowerLegLength]; float sqrLowerLeg = lowerLeg * lowerLeg; float cosUpperLeg = (sqrUpperLeg + sqrLength - sqrLowerLeg) / (2 * upperLeg * length); float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg); // clip for the case that target position is not reachable const Range<float> clipping(-1.0f, 1.0f); bool reachable = true; if(!clipping.isInside(cosKnee) || clipping.isInside(upperLeg)) { cosKnee = clipping.limit(cosKnee); cosUpperLeg = clipping.limit(cosUpperLeg); reachable = false; } float joint1 = target.translation.z == 0.0f ? 0.0f : atanf(target.translation.y / -target.translation.z) * sign; float joint2 = -acos(cosUpperLeg); joint2 -= atan2(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs() * -sgn(target.translation.z)); float joint3 = M_PI - acos(cosKnee); RotationMatrix beforeFoot = RotationMatrix().rotateX(joint1 * sign).rotateY(joint2 + joint3); joint1 -= M_PI_4; // because of the strange hip of Nao // compute joints from rotation matrix using theorem of euler angles // see http://www.geometrictools.com/Documentation/EulerAngles.pdf // this is possible because of the known order of joints (y, x, z) where z is left open and is seen as failure RotationMatrix foot = beforeFoot.invert() * target.rotation; float joint5 = asin(-foot[2].y) * -sign * -1; float joint4 = -atan2(foot[2].x, foot[2].z) * -1; //float failure = atan2(foot[0].y, foot[1].y) * sign; // set computed joints in jointAngles jointAngles[firstJoint + 0] = joint0; jointAngles[firstJoint + 1] = joint1; jointAngles[firstJoint + 2] = joint2; jointAngles[firstJoint + 3] = joint3; jointAngles[firstJoint + 4] = joint4; jointAngles[firstJoint + 5] = joint5; return reachable; }
void pendulum_menu() { cleardevice(); outtext((char*)"Click and drag mouse to define anchor point and starting point for pendulum"); do { mouseInput(x1, y1, x2, y2); if (y2 < y1) outtextxy(0, 20, (char*)"Please choose an anchor point higher than the starting point of pendulum"); }while(y2 < y1); int l = (int) (sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2)) + 0.5); Point2D a(x1, y1); float ang = atanf(((float)(x2-x1))/(y2-y1)); cleardevice(); pendulum(ang, l, a, 25); }
const Entity EntityBuilder::CreateHealingLight(const XMFLOAT3 & pos, const XMFLOAT3 & rot, const DirectX::XMFLOAT3 & color, float intensity, float outerAngle, float innerAngle, float range) { Entity ent = _entity.Create(); _transform->CreateTransform(ent); _light->BindSpotLight(ent, color, intensity, outerAngle, innerAngle, range); _light->SetAsVolumetric(ent, true); _transform->SetPosition(ent, pos); _transform->SetRotation(ent, rot); float s = range*atanf(outerAngle); _transform->SetScale(ent, XMFLOAT3(s, s, range)); return ent; }
inline void CompressedRectilinearPortraitProjector::mapBackward(float u, float v, float &x, float &y) { u /= - scale; v /= scale; float aatg = a * atanf(u / a); float u_ = aatg; float v_ = atanf(v * cosf( aatg ) / b); float cosv = cosf(v_); float y_ = cosv * sinf(u_); float x_ = sinf(v_); float z_ = cosv * cosf(u_); float z; x = k_rinv[0] * x_ + k_rinv[1] * y_ + k_rinv[2] * z_; y = k_rinv[3] * x_ + k_rinv[4] * y_ + k_rinv[5] * z_; z = k_rinv[6] * x_ + k_rinv[7] * y_ + k_rinv[8] * z_; if (z > 0) { x /= z; y /= z; } else x = y = -1; }
float VectorGetYaw(vector_t *arg){ float yaw; /* figure out what quadrant we're in, react accordingly */ if(arg->z > 0){ if(arg->x > 0){ /* quadrant 1 */ yaw = atanf(arg->z/arg->x) + M_PI; } else{ /* quadrant 2 */ yaw = (atanf(arg->z/arg->x) + M_PI_2) + M_PI; } } else{ if(arg->x < 0){ /* quadrant 3 */ yaw = (atanf(arg->z/arg->x) + M_PI) + M_PI; } else{ /* quadrant 4 */ yaw = (M_2PI - atanf(arg->z/arg->x)) + M_PI; } } return yaw; }
float3 EffectTranslucency::ComputePhase( const float3& _Anisotropy, int _PixelDistance, int _SamplesCount, float _TexelSize, float _SliceThickness ) { // Imagine receiving light from a point above you offset by a distance d // // 0 Source // -------------------*--------*------------- // | / // | / // | / // |/ // -------------------*---------------------- // Receiver // // The source and receiver make an angle with the vertical, diffusion direction. // This is that angle we compute the phase for. // float Theta = atanf( _PixelDistance * _TexelSize / _SliceThickness ); float CosTheta = cosf( Theta ); float3 P; P.x = (1.0f - _Anisotropy.x*_Anisotropy.x) * powf( 1.0f + _Anisotropy.x*_Anisotropy.x - 2.0f*_Anisotropy.x*CosTheta, -1.5f ); P.y = (1.0f - _Anisotropy.y*_Anisotropy.y) * powf( 1.0f + _Anisotropy.y*_Anisotropy.y - 2.0f*_Anisotropy.y*CosTheta, -1.5f ); P.z = (1.0f - _Anisotropy.z*_Anisotropy.z) * powf( 1.0f + _Anisotropy.z*_Anisotropy.z - 2.0f*_Anisotropy.z*CosTheta, -1.5f ); P = INV4PI * P; // Normalize over hemisphere // Now, compute the weight to give this phase function // The weight is simply the solid angle covered by all the texels using the phase function // This solid angle is a spherical band between current and next pixel angles, divided by the amount of samples taken in that band // float NextTheta = atanf( (1+_PixelDistance) * _TexelSize / _SliceThickness ); float CosNextTheta = cosf( NextTheta ); float SolidAngle = TWOPI * (CosTheta - CosNextTheta); // Solid angle covered by the spherical band in [Theta,NextTheta] SolidAngle /= _SamplesCount; // Solid angle of a single sample in that band return SolidAngle * P; }
/** * lw6sys_math_angle_360 * * @sys_context: global system context * @x: x coordinate * @y: y coordinate * * This is a wrapper over the standard @atan function which will * handle internally the special x == 0 case and the various positive/negative * values of @x and @y. * * Return value: the angle, in degrees */ float lw6sys_math_angle_360 (lw6sys_context_t * sys_context, int x, int y) { float ret = 0.0f; if (x == 0) { ret = (y == 0) ? 0.0f : (y > 0) ? 90.0f : 270.0f; } else { if (x > 0) { if (y >= 0) { ret = lw6sys_math_rad2deg (atanf (((float) y) / ((float) x))); } else { ret = 360.0f - lw6sys_math_rad2deg (atanf (((float) -y) / ((float) x))); } } else { if (y >= 0) { ret = 180.0f - lw6sys_math_rad2deg (atanf (((float) y) / ((float) -x))); } else { ret = 180.0f + lw6sys_math_rad2deg (atanf (((float) -y) / ((float) -x))); } } } return ret; }
void listenToCommands() { initCOMPort(); initUSBRadio(); lastSpeedSentTimer.start(); HANDLE waitHandles[3] = { startSignal, osStatusDongle.hEvent }; stateGoal = goalOnRight; stateBallSeen = ballOnRight; while (true) { float floorX, floorY; int currentx, currenty; findNearestFloorBall(floorX, floorY, currentx, currenty); float angle = atanf(floorY / floorX) * 180.0 / PI; swprintf_s(buffer, L"NX %.2f \n NY %.2f\n ang %.2f", floorX, floorY, angle); SetWindowText(infoGUI, buffer); swprintf_s(buffer, L"greencount %d", fieldGreenPixelCountShare); SetWindowText(infoGUI3, buffer); if (hCOMDongle != INVALID_HANDLE_VALUE) { if (lastSpeedSentTimer.time() > 0.5) { //if we send the speeds too often we get communication timeouts setSpeedBoth(currentDrivingState); lastSpeedSentTimer.start(); } } switch (WaitForMultipleObjects(2, waitHandles, FALSE, 50)) { //case WAIT_OBJECT_0: // prints(L"radio event %X\n", dwCommEvent); // //receiveCommand(); // //WaitCommEvent(hCOMRadio, &dwCommEvent, &osStatus); // break; case WAIT_OBJECT_0: prints(L"Start signal arrived.\n"); SetWindowText(stateStatusGUI, L"started"); dribblerON(); play(); dribblerOFF(); //discharge(); break; case WAIT_OBJECT_0 + 1: //info from the main board controller //prints(L"main board COM event %X\n", dwCommEventDongle); handleMainBoardCommunication(); WaitCommEvent(hCOMDongle, &dwCommEventDongle, &osStatusDongle); //listen to new events, ie beginning character break; } receiveCommand(); } }
void Stage04::StagePass() { this->unschedule(SEL_SCHEDULE(&Stage04::createCarWithTimer)); bus = Sprite::create("bus.png"); bus->setPosition(winSize.width + 400, 150); this->addChild(bus); DelayTime* busdelaytime1 = DelayTime::create(1); MoveTo* busmoveto1 = MoveTo::create(3,ccp(winSize.width / 2, 150)); DelayTime* busdelaytime2 = DelayTime::create(2); MoveTo* busmoveto2 = MoveTo::create(3,ccp(-400, 150)); auto busaction = Sequence::create(busdelaytime1,busmoveto1, busdelaytime2, busmoveto2, NULL); bus->runAction(busaction); auto s = getChildByTag(1); float o = winSize.width / 2 - s->getPosition().x; float a = 100 - s->getPosition().y; float at = (float) CC_RADIANS_TO_DEGREES( atanf( o/a) ); if( a < 0 ) { if( o < 0 ) at = 180 + fabs(at); else at = 180 - fabs(at); } RotateTo* rotateto = RotateTo::create(0.5, at); DelayTime* herodelaytime = DelayTime::create(4); MoveTo* heromoveto = MoveTo::create(1,ccp(winSize.width / 2, 150)); FadeOut* herofadeout = FadeOut::create(0.5f); auto heroaction = Sequence::create(rotateto,herodelaytime, heromoveto, herofadeout, NULL); s->runAction(heroaction); winboard = Sprite::create("winboard.png"); winboard->setPosition(winSize.width / 2, winSize.height + 250); this->addChild(winboard); LabelTTF* playTTF1 = LabelTTF::create("next stage","ARNPRIOR",32); MenuItemLabel* item1 = MenuItemLabel::create(playTTF1,CC_CALLBACK_1(Stage04::WinBoardNextCallBack,this)); LabelTTF* playTTF2 = LabelTTF::create("main menu","ARNPRIOR",32); MenuItemLabel* item2 = MenuItemLabel::create(playTTF2,CC_CALLBACK_1(Stage04::WinBoardMainMenuCallBack,this)); item1->setPositionY(item1->getPositionY() + 20); item2->setPositionY(item2->getPositionY() - 20); Menu* playMenu = Menu::create(item1, item2, NULL); playMenu->setPosition(270,130); winboard->addChild(playMenu); DelayTime* boarddelaytime = DelayTime::create(7); MoveTo* boardmoveto = MoveTo::create(1,ccp(winSize.width / 2, winSize.height / 2)); auto boardaction = Sequence::create(boarddelaytime, boardmoveto, NULL); winboard->runAction(boardaction); }
//--------------------------- // //--------------------------- Matrix44 Matrix44::GetBillboardX() const { static Matrix44 matWorld; static Vector3 vDir; vDir.x = _13; vDir.y = _23; vDir.z = _33; if( vDir.x == 0.0F ) { matWorld.SetIdentity(); } else if( vDir.x > 0.0F ) { matWorld.SetRotationY( -atanf( vDir.z / vDir.x ) + MATH_PI / 2 ); } else { matWorld.SetRotationY( -atanf( vDir.z / vDir.x ) - MATH_PI / 2 ); } //if..else if..else.. return matWorld; } //Matrix44::GetBillboardX
crlb_qi_run(const vector2 *anchor, const size_t count, const vector2 *location) { const float pi = (float) M_PI; // speed of light in su / s, assumes su = 1dm const float c = 299792458.0f / crlb_qi_su; // Constant alpha of the paper const float alpha = (c * c) / (8.0f * pi * pi * crlb_qi_beta * crlb_qi_beta); // Calculate the CRLB float numer = 0.0f; float denom = 0.0f; for (size_t i = 0; i < count; i++) { numer += distance_v(&(anchor[i]), location); for (size_t j = 0; j < i; j++) { float phi_i = atanf((location->y - anchor[i].y) / (location->x - anchor[i].x)); float phi_j = atanf((location->y - anchor[j].y) / (location->x - anchor[j].x)); float a = sinf(phi_i - phi_j); denom += distance_v(location, &(anchor[i])) * distance_v(location, &(anchor[j])) * a * a; } } return crlb_qi_scale * alpha * numer / denom; }
void Dart::setAttributes(position3D startPos, position3D destPos, float power) { this->x = startPos.x; this->y = startPos.y; this->z = startPos.z; this->destx = destPos.x; this->desty = destPos.y; this->destz = destPos.z; this->dx = (destx - x)/30; this->dy = (desty - y)/30; this->dz = (destz - z)/30; this->angleX = 0.0; this->angleY = 0.0; this->angleZ = 0.0; draw = false; flies = true; justHit = false; angleX = atanf(dx/dz)*radToDec; angleY = atanf(dy/dz)*radToDec; }
float complex catanhf(float complex z) { float x, y, ax, ay, rx, ry; x = crealf(z); y = cimagf(z); ax = fabsf(x); ay = fabsf(y); if (y == 0 && ax <= 1) return (CMPLXF(atanhf(x), y)); if (x == 0) return (CMPLXF(x, atanf(y))); if (isnan(x) || isnan(y)) { if (isinf(x)) return (CMPLXF(copysignf(0, x), y + y)); if (isinf(y)) return (CMPLXF(copysignf(0, x), copysignf(pio2_hi + pio2_lo, y))); return (CMPLXF(x + 0.0L + (y + 0), x + 0.0L + (y + 0))); } if (ax > RECIP_EPSILON || ay > RECIP_EPSILON) return (CMPLXF(real_part_reciprocal(x, y), copysignf(pio2_hi + pio2_lo, y))); if (ax < SQRT_3_EPSILON / 2 && ay < SQRT_3_EPSILON / 2) { raise_inexact(); return (z); } if (ax == 1 && ay < FLT_EPSILON) rx = (m_ln2 - logf(ay)) / 2; else rx = log1pf(4 * ax / sum_squares(ax - 1, ay)) / 4; if (ax == 1) ry = atan2f(2, -ay) / 2; else if (ay < FLT_EPSILON) ry = atan2f(2 * ay, (1 - ax) * (1 + ax)) / 2; else ry = atan2f(2 * ay, (1 - ax) * (1 + ax) - ay * ay) / 2; return (CMPLXF(copysignf(rx, x), copysignf(ry, y))); }
/* ==================== CalcFov ==================== */ float CalcFov (float fov_x, float width, float height) { float a; float x; if (fov_x < 1 || fov_x > 179) Sys_Error ("Bad fov: %f", fov_x); x = width/tanf(fov_x/360*Q_PI); a = atanf(height/x); a = a*360/Q_PI; return a; }
static void makect_32(void) { float *c = rdft_w + 32; const int nc = 32; int j, nch; float delta; ip[1] = nc; nch = nc >> 1; delta = atanf(1.0f) / nch; c[0] = cosf(delta * nch); c[nch] = 0.5f * c[0]; for (j = 1; j < nch; j++) { c[j] = 0.5f * cosf(delta * j); c[nc - j] = 0.5f * sinf(delta * j); } }
void quat_logdif(AD_Quaternion *q1, AD_Quaternion *q2, AD_Quaternion *q3) { AD_Quaternion inv, dif; float len, len1, s; quat_inverse (q1, &inv); quat_mul (&inv, q2, &dif); len = sqrtf (dif.x*dif.x + dif.y*dif.y + dif.z*dif.z); s = quat_dot (q1, q2); if (s != 0.0) len1 = atanf (len / s); else len1 = (float)(M_PI/2.0f); if (len != 0.0) len1 /= len; q3->w = 0.0; q3->x = dif.x * len1; q3->y = dif.y * len1; q3->z = dif.z * len1; }
void rotation() { pmath::Vec2f angVec = m_rigidBody->GetPosition() - prevPos; if (m_direction * m_rigidBody->GetPosition().x > 0) { m_sliding = 1.8; } else { m_sliding = 1; } angle = m_sliding * atanf(angVec.y / angVec.x); parent->transform.SetRotation(pmath::radiansToDegrees(angle)); }
OSPTexture2D getOSPDepthTextureFromOpenGLPerspective() { // compute fovy, aspect, zNear, zFar from OpenGL projection matrix GLdouble glProjectionMatrix[16]; glGetDoublev(GL_PROJECTION_MATRIX, glProjectionMatrix); const double m0 = glProjectionMatrix[0]; const double m5 = glProjectionMatrix[5]; const double m10 = glProjectionMatrix[10]; const double m14 = glProjectionMatrix[14]; const double k = (m10 - 1.0f) / (m10 + 1.0f); const double fovy = 2. * atanf(1.0f / m5) * 180./M_PI; const double aspect = m5 / m0; const double zNear = (m14 * (1.0f - k)) / (2.0f * k); const double zFar = k * zNear; // get camera direction and up vectors from model view matrix GLdouble glModelViewMatrix[16]; glGetDoublev(GL_MODELVIEW_MATRIX, glModelViewMatrix); const ospray::vec3f cameraUp( glModelViewMatrix[1], glModelViewMatrix[5], glModelViewMatrix[9]); const ospray::vec3f cameraDir(-glModelViewMatrix[2], -glModelViewMatrix[6], -glModelViewMatrix[10]); // get window dimensions from OpenGL viewport GLint glViewport[4]; glGetIntegerv(GL_VIEWPORT, glViewport); const size_t width = glViewport[2]; const size_t height = glViewport[3]; // read OpenGL depth buffer float *glDepthBuffer = new float[width * height]; glReadPixels(0, 0, width, height, GL_DEPTH_COMPONENT, GL_FLOAT, (GLvoid *)glDepthBuffer); // get an OSPRay depth texture from the OpenGL depth buffer OSPTexture2D depthTexture = getOSPDepthTextureFromOpenGLPerspective(fovy, aspect, zNear, zFar, (osp::vec3f&)cameraDir, (osp::vec3f&)cameraUp, glDepthBuffer, width, height); // free allocated depth buffer delete[] glDepthBuffer; // return OSPRay depth texture return depthTexture; }
void BWJumpBy::update(float t) { // parabolic jump (since v0.8.2) if (m_pTarget) { float frac = fmodf( t * m_nJumps, 1.0f ); float y = m_height * 4 * frac * (1 - frac); // float fA = m_height * 4 ; // float y = - frac*frac * fA + fA * frac + 5; y += m_delta.y * t; float x = m_delta.x * t; #if CC_ENABLE_STACKABLE_ACTIONS CCPoint currentPos = m_pTarget->getPosition(); CCPoint diff = ccpSub( currentPos, m_previousPos ); m_startPosition = ccpAdd( diff, m_startPosition); CCPoint newPos = ccpAdd( m_startPosition, ccp(x,y)); m_pTarget->setPosition(newPos); m_previousPos = newPos; if(m_bRotHead) { CCPoint dirPoint = ccpSub( currentPos,newPos ); float fRadian = atanf(dirPoint.y/dirPoint.x); if((dirPoint.y > 0 && dirPoint.x > 0) || (dirPoint.y < 0 && dirPoint.x > 0)) { fRadian = M_PI_2 - fRadian; } else if((dirPoint.y < 0 && dirPoint.x < 0)|| (dirPoint.y > 0 && dirPoint.x < 0)) { fRadian = -M_PI_2 - fRadian; } float m_fRotation = fRadian*180.0/M_PI; m_pTarget->setRotation(m_fRotation); } #else m_pTarget->setPosition(ccpAdd( m_startPosition, ccp(x,y))); #endif // !CC_ENABLE_STACKABLE_ACTIONS } }
/** Navigates around (x, y). Clockwise iff radius > 0 */ void nav_circle_XY(float x, float y, float radius) { struct EnuCoor_f *pos = stateGetPositionEnu_f(); float last_trigo_qdr = nav_circle_trigo_qdr; nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x); float sign_radius = radius > 0 ? 1 : -1; if (nav_in_circle) { float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr; NormRadAngle(trigo_diff); nav_circle_radians += trigo_diff; trigo_diff *= - sign_radius; if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius nav_circle_radians_no_rewind += trigo_diff; } } float dist2_center = DistanceSquare(pos->x, pos->y, x, y); float dist_carrot = CARROT * NOMINAL_AIRSPEED; radius += -nav_shift; float abs_radius = fabs(radius); /** Computes a prebank. Go straight if inside or outside the circle */ circle_bank = (dist2_center > Square(abs_radius + dist_carrot) || dist2_center < Square(abs_radius - dist_carrot)) ? 0 : atanf(stateGetHorizontalSpeedNorm_f() * stateGetHorizontalSpeedNorm_f() / (NAV_GRAVITY * radius)); float carrot_angle = dist_carrot / abs_radius; carrot_angle = Min(carrot_angle, M_PI / 4); carrot_angle = Max(carrot_angle, M_PI / 16); float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle; horizontal_mode = HORIZONTAL_MODE_CIRCLE; float radius_carrot = abs_radius; if (nav_mode == NAV_MODE_COURSE) { radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius); } fly_to_xy(x + cosf(alpha_carrot)*radius_carrot, y + sinf(alpha_carrot)*radius_carrot); nav_in_circle = true; nav_circle_x = x; nav_circle_y = y; nav_circle_radius = radius; }
// inverse kinematics // helper functions, calculates angle theta1 (for YZ-pane) int RotaryDeltaSolution::delta_calcAngleYZ(float x0, float y0, float z0, float &theta) { float y1 = -0.5F * tan30 * delta_f; // f/2 * tan 30 y0 -= 0.5F * tan30 * delta_e; // shift center to edge // z = a + b*y float a = (x0 * x0 + y0 * y0 + z0 * z0 + delta_rf * delta_rf - delta_re * delta_re - y1 * y1) / (2.0F * z0); float b = (y1 - y0) / z0; float d = -(a + b * y1) * (a + b * y1) + delta_rf * (b * b * delta_rf + delta_rf); // discriminant if (d < 0.0F) return -1; // non-existing point float yj = (y1 - a * b - sqrtf(d)) / (b * b + 1.0F); // choosing outer point float zj = a + b * yj; theta = 180.0F * atanf(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0F : 0.0F); return 0; }
/** Used in \ref determineOvertakePosition to adjust the overtake position * which is calculated by slope of line if it's too close. * \param old_slope Old slope calculated. * \param rotate_up If adjust the slope upwards. * \return A newly calculated slope. */ float SoccerAI::rotateSlope(float old_slope, bool rotate_up) { const float theta = atanf(old_slope) + (old_slope < 0 ? M_PI : 0); float new_theta = theta + (rotate_up ? M_PI / 6 : -M_PI /6); if (new_theta > ((M_PI / 2) - 0.02f) && new_theta < ((M_PI / 2) + 0.02f)) { // Avoid almost tan 90 new_theta = (M_PI / 2) - 0.02f; } // Check if over-rotated if (new_theta > M_PI) new_theta = M_PI - 0.1f; else if (new_theta < 0) new_theta = 0.1f; return tanf(new_theta); } // rotateSlope
void Helm::goTo( xyz location ) { xyz delta; float distance; float theta; delta.x = location.x - currentPos.x; delta.y = location.y - currentPos.y; delta.z = location.z - currentPos.z; distance = sqrt( pow(delta.x, 2) + pow(delta.y, 2) ); theta = atanf( delta.x / delta.y ) - this->rotation; rotate( theta ); goDistance( distance ); }
/* ==================== AdaptFovx Adapts a 4:3 horizontal FOV to the current screen size (Hor+). ==================== */ static float AdaptFovx (float fov43, float w, float h) { static const float pi = M_PI; /* float instead of double. */ if (w <= 0 || h <= 0) return fov43; /* * Formula: * * fov = 2.0 * atan(width / height * 3.0 / 4.0 * tan(fov43 / 2.0)) * * The code below is equivalent but precalculates a few values and * converts between degrees and radians when needed. */ return (atanf(tanf(fov43 / 360.0f * pi) * (w / h) * 0.75f) / pi * 360.0f); }
void ani_subobject_state::set_affine_transform(const ani_affine_transform& affine_transform) { m_scale.x = sqrtf((affine_transform.a * affine_transform.a) + (affine_transform.c * affine_transform.c)); m_scale.y = sqrtf((affine_transform.b * affine_transform.b) + (affine_transform.d * affine_transform.d)); m_position.x = affine_transform.tx; m_position.y = affine_transform.ty; f32 sign = atanf(-affine_transform.c / affine_transform.a); f32 radians = acosf(affine_transform.a / m_scale.x); m_rotation = glm::degrees(radians); if ((m_rotation > 90.f && sign > 0.f) || (m_rotation < 90.f && sign < 0.f)) { m_rotation = 360.f - m_rotation; } }