/// 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;
}
}
