int CDanmakuCircleLine::Fire()
{
for( int i = 0; i < n; ++i ) {
double nn = (double)(n-1) / 2.0;
double a;
if( n != 0 )
a = this->offset + w * (double)i/ n;
else
a = this->offset + 180;
for ( int j = 0; j < m; ++j ) {
CTama *p = pTama->Copy();
if( m != 1 )
p->v = this->sv + j * ( this->ev - this->sv ) / ( this->m - 1 );
else
p->v = this->sv;
p->a = a;
double xx = p->v * Cos( p->a ) + vv * Cos( aa );
double yy = p->v * Sin( p->a ) + vv * Sin( aa );
p->a = Atan2( xx, yy );
p->v = sqrt( pow2( xx ) + pow2( yy ));
g_lTama.Add( p );
}
}
return 1;
}
bool Quaternion::setLog(const Quaternion& q, int n)
{
Real mag = q.norm();
Real immag = q.imNorm();
if(immag == Zero)
{
//there are infinitely (uncountably) many such logarithms, just choose the real case
if(mag == Zero)
return false;
w = Log(mag);
x = Zero;
y = Zero;
z = Zero;
}
else
{
Real immaginv = One / immag;
Real arg = Atan2(immag, q.w) + Two*Pi*n;
w = Log(mag);
x = immaginv * arg * q.x;
y = immaginv * arg * q.y;
z = immaginv * arg * q.z;
}
return true;
}
void co_XyzToEq(double xyz[], double *ra, double *dec)
{
double r,rho,x,y,z;
x = xyz[0];
y = xyz[1];
z = xyz[2];
rho = sqrt(x * x + y * y);
r = sqrt(rho * rho + z * z);
if (r < 1.0e-20)
{
*ra = 0.0;
*dec = 0.0;
}
else
{
*ra = Atan2 (y,x) * 12.0 / M_PI;
if (fabs(rho) > fabs(z))
*dec = atan(z/rho) * 180.0 / M_PI;
else
{
if (z > 0.0)
*dec = 90.0 - atan(rho/z) * 180.0 / M_PI;
else
*dec = -90.0 - atan(rho/z) * 180.0 / M_PI;
}
}
}
void SideStrafeGeneral(const PlayerData& player, const MovementVars& vars, bool onground, double wishspeed,
const StrafeButtons& strafeButtons, bool useGivenButtons, Button& usedButton, double vel_yaw, double theta, bool right, Vector2D& velocity, double& yaw)
{
if (useGivenButtons) {
if (!onground) {
if (right)
usedButton = strafeButtons.AirRight;
else
usedButton = strafeButtons.AirLeft;
} else {
if (right)
usedButton = strafeButtons.GroundRight;
else
usedButton = strafeButtons.GroundLeft;
}
} else {
usedButton = GetBestButtons(theta, right);
}
double phi = ButtonsPhi(usedButton);
theta = right ? -theta : theta;
if (!player.Velocity.AsVector2D().IsZero(0))
vel_yaw = Atan2(player.Velocity.y, player.Velocity.x);
yaw = NormalizeRad(vel_yaw - phi + theta);
Vector2D avec(std::cos(yaw + phi), std::sin(yaw + phi));
PlayerData pl = player;
VectorFME(pl, vars, onground, wishspeed, avec);
velocity = pl.Velocity.AsVector2D();
}
int CEnemyFairy16::Run()
{
const v = 2;
if( count < 60 ) {
x -= v;
}
else if( count < 240 ) {
if( count%10 == 0) {
CTamaNormal tama;
tama.x = GetX();
tama.y = GetY();
tama.type = 13;
tama.color = 0;
tama.v = 1.0 + 0.3 * GetDifficult();
CDanmakuHorizon d(&tama);
d.w = 1;
if( GetDifficult() == DIFF_LUNATIC) {
d.n = 4;
d.m = 4;
}else {
d.n = 1 * ( GetDifficult() + 1 );
d.m = 1 * ( GetDifficult() + 1 );
}
double angle = Atan2( g_pPlayer->GetX() - GetX(), g_pPlayer->GetY() - GetY());
d.offset = angle + 360/d.m/4;
d.Fire();
g_pMaterial->sndFire.Play(0);
}
}
else{
x += v;
}
return CEnemyBase::Run();
}
void HormingShot::update(Game* game) {
Shot::update(game);
accel += 0.2;
const Vec2 targetPos = game->getNearEnemyPos();
const double rad2 = Atan2(targetPos.y - pos.y, targetPos.x - pos.x);
const double radLimit = Radians(10);
if (count < TwoPi) {
if (Abs(rad2 - rad) < radLimit) {
rad = rad2;
count += rad2;
} else {
if (rad2 < rad - Pi) {
rad -= TwoPi;
} else if (rad2 > rad + Pi) {
rad += TwoPi;
}
if (rad2 < rad) {
rad -= radLimit;
} else {
rad += radLimit;
}
count += radLimit;
}
}
pos += Vec2(Cos(rad), Sin(rad)) * (10.0 + accel);
tracks.push_front(pos);
if (tracks.size() > 15) tracks.pop_back();
}
int CDanmakuSegment::Fire()
{
double vx1 = v1 * Cos( a1 );
double vy1 = v1 * Sin( a1 );
double vx2 = v2 * Cos( a2 );
double vy2 = v2 * Sin( a2 );
double nn = n - 1;
double vx,vy;
for( int i = 0; i < n; ++i ) {
if( n != 1 ) {
vx = vx1 * ( nn - i ) / nn + vx2 * i / nn;
vy = vy1 * ( nn - i ) / nn + vy2 * i / nn;
}
else {
vx = (vx1 + vx2)/2;
vy = (vy1 + vy2)/2;
}
double vv = sqrt(pow2(vx) + pow2(vy));
double aa = Atan2(vx,vy);
CTama *p = pTama->Copy();
p->v = pTama->v * vv;
p->a = aa;
g_lTama.Add( p );
}
return 1;
}
Real HollowBall::closestPoint(const AxisSweptPoint& p) const
{
Vector3 x,y;
GetCanonicalBasis(p.axis.direction,x,y);
Vector2 c2,p2,s2;
c2.x = x.dot(center);
c2.y = y.dot(center);
p2.x = x.dot(p.p);
p2.y = y.dot(p.p);
s2.x = x.dot(p.axis.source);
s2.y = y.dot(p.axis.source);
c2-=s2;
p2-=s2;
Real ac = Atan2(c2.y,c2.x);
Real ap = Atan2(p2.y,p2.x);
return ac-ap;
}
M_Real
M_VectorVecAngle2_FPU(M_Vector2 vOrig, M_Vector2 vOther)
{
M_Vector2 vd;
vd = M_VectorSub2p_FPU(&vOther, &vOrig);
return (Atan2(vd.y, vd.x));
}
/* Compute minimal distance from a circle to a point p. */
M_Real
M_CirclePointDistance2(M_Circle2 C, M_Vector2 p)
{
M_Vector2 vR = M_VecSub2(p, C.p);
M_Real theta = Atan2(vR.y, vR.x);
return M_VecDistance2(p, M_VECTOR2(C.p.x + C.r*Cos(theta),
C.p.y + C.r*Sin(theta)));
}
Quat::Quat(const Vec3f &V1,const Vec3f &V2,const Vec3f &V3)
{
Quat aQuat=Quat(V2,V1);
Float ang=Atan2(V1.x,V1.z);
Quat aQuat2=Quat(ang,V3);
*this=aQuat*aQuat2;
}
double MaxAccelIntoYawTheta(const PlayerData& player, const MovementVars& vars, bool onground, double wishspeed, double vel_yaw, double yaw)
{
if (!player.Velocity.AsVector2D().IsZero(0))
vel_yaw = Atan2(player.Velocity.y, player.Velocity.x);
double theta = MaxAccelTheta(player, vars, onground, wishspeed);
if (theta == 0.0 || theta == M_PI)
return NormalizeRad(yaw - vel_yaw + theta);
return std::copysign(theta, NormalizeRad(yaw - vel_yaw));
}
void Player::update(Game* game) {
frameCount++;
auto pad = XInput(0);
pad.setLeftThumbDeadZone();
pad.setRightThumbDeadZone();
//move
if (!Vec2(pad.leftThumbX, pad.leftThumbY).isZero) {
rad = Atan2(-pad.leftThumbY, pad.leftThumbX);
pos += Vec2(Cos(rad), Sin(rad)) * 7.5;
}
pos = Vec2(Clamp(pos.x, 0.0, static_cast<double>(Game::stageSize.x)), Clamp(pos.y, 0.0, static_cast<double>(Game::stageSize.y)));
//bomb
if (Input::KeyX.clicked) {
game->getBulletManager()->clear();
game->getEnemyManager()->clear();
}
//fire
if (!Vec2(pad.rightThumbX, pad.rightThumbY).isZero) {
for (int i : {-1, 1, 0}) {
const double fireRad = Atan2(-pad.rightThumbY, pad.rightThumbX) + Radians(5 * i);
if (fireCount % 5 == 0) {
auto shot = std::make_shared<NormalShot>(pos, fireRad);
shotManager->add(shot);
}
}
}
fireCount++;
shotManager->update(game);
tracks.push_front(pos);
if (tracks.size() > 20) tracks.pop_back();
damageCount++;
checkBulletHit(game);
if (frameCount % 10 == 0 && damageCount) shield++;
shield = Clamp(shield, 0, SHIELD_MAX);
hp = Clamp(hp, 0, HP_MAX);
}
int CDanmakuCircleToPlayer::Fire()
{
double angle = Atan2( g_pPlayer->GetX() - pTama->x, g_pPlayer->GetY() - pTama->y);
for( int i = 0; i < n; ++i ) {
double nn = (double)(n-1) / 2.0;
double a;
if( n != 0 )
a = this->offset + w * (double)i/ n;
else
a = this->offset + 180;
CTama *p = pTama->Copy();
p->a = a;
double xx = p->v * Cos( p->a ) + vv * Cos( aa + angle );
double yy = p->v * Sin( p->a ) + vv * Sin( aa + angle );
p->a = Atan2( xx, yy );
p->v = sqrt( pow2( xx ) + pow2( yy ));
g_lTama.Add( p );
}
return 1;
}
int CDanmakuToPlayerRandom::Fire()
{
double angle = Atan2( g_pPlayer->GetX() - pTama->x, g_pPlayer->GetY() - pTama->y);
for( int i = 0; i < n; ++i ) {
double a = angle + rand( -w, w ) + offset;
double v = rand( sv, ev );
CTama *p = pTama->Copy();
p->a = a;
p->v = v;
g_lTama.Add( p );
}
return 1;
}
void MiddleEnemy::update(Game* game) {
Super::update(game);
rad += Radians(10.0);
const Vec2 playerPos = game->getPlayer()->getPos();
const double rad2 = Atan2(playerPos.y - pos.y, playerPos.x - pos.x);
if (frameCount % 100 < 50) {
pos += Vec2(Cos(rad2), Sin(rad2)) * 3.0;
} else if (frameCount % 3 == 0) {
const double fireRad = rad2 + Radians(Random(-15.0, 15.0));
auto bullet = std::make_shared<Bullet>(pos, Color(255, 100, 100), fireRad, Random(2.0, 7.0), 0.0);
game->getBulletManager()->add(bullet);
}
}
static void
UpdateHue(AG_HSVPal *pal, int x, int y)
{
float h;
h = Atan2((float)y, (float)x);
if (h < 0) {
h += (float)(2*AG_PI);
}
AG_SetFloat(pal, "hue", h/(2*AG_PI)*360.0);
UpdatePixelFromHSVA(pal);
AG_PostEvent(NULL, pal, "h-changed", NULL);
pal->flags |= AG_HSVPAL_DIRTY;
AG_Redraw(pal);
}
static float
PointProximity(void *p, VG_View *vv, VG_Vector *vPt)
{
VG_Circle *vc = p;
VG_Vector vCenter = VG_Pos(vc->p);
float theta = Atan2(vPt->y - vCenter.y,
vPt->x - vCenter.x);
VG_Vector vNear;
float d;
vNear.x = vCenter.x + vc->r*Cos(theta);
vNear.y = vCenter.y + vc->r*Sin(theta);
d = VG_Distance(*vPt, vNear);
*vPt = vNear;
return (d);
}
int CDanmakuToPlayer::Fire()
{
double angle = Atan2( g_pPlayer->GetX() - pTama->x, g_pPlayer->GetY() - pTama->y);
for( int i = 0; i < n; ++i ) {
double m = (double)(n-1) / 2.0;
double a;
if( m != 0 )
a = angle + w * ( (double)i - m) / m;
else
a = angle;
CTama *p = pTama->Copy();
p->a = a;
g_lTama.Add( p );
}
return 1;
}
double YawStrafeMaxAngle(PlayerData& player, const MovementVars& vars, bool onground, double wishspeed, const StrafeButtons& strafeButtons, bool useGivenButtons, Button& usedButton,
double vel_yaw, double yaw)
{
bool safeguard_yaw;
double theta = MaxAngleTheta(player, vars, onground, wishspeed, safeguard_yaw);
if (!player.Velocity.AsVector2D().IsZero(0.0f))
vel_yaw = Atan2(player.Velocity[1], player.Velocity[0]);
Vector2D newvel;
double resulting_yaw;
SideStrafeGeneral(player, vars, onground, wishspeed, strafeButtons, useGivenButtons, usedButton, vel_yaw, theta, (NormalizeRad(yaw - vel_yaw) < 0), newvel, resulting_yaw);
if (safeguard_yaw) {
Vector2D test_vel1, test_vel2;
double test_yaw1, test_yaw2;
SideStrafeGeneral(player, vars, onground, wishspeed, strafeButtons, useGivenButtons, usedButton, vel_yaw, min(theta - SAFEGUARD_THETA_DIFFERENCE_RAD, 0.0), (NormalizeRad(yaw - vel_yaw) < 0), test_vel1, test_yaw1);
SideStrafeGeneral(player, vars, onground, wishspeed, strafeButtons, useGivenButtons, usedButton, vel_yaw, std::max(theta + SAFEGUARD_THETA_DIFFERENCE_RAD, 0.0), (NormalizeRad(yaw - vel_yaw) < 0), test_vel2, test_yaw2);
double cos_test1 = test_vel1.Dot(player.Velocity.AsVector2D()) / (player.Velocity.Length2D() * test_vel1.Length());
double cos_test2 = test_vel2.Dot(player.Velocity.AsVector2D()) / (player.Velocity.Length2D() * test_vel2.Length());
double cos_newvel = newvel.Dot(player.Velocity.AsVector2D()) / (player.Velocity.Length2D() * newvel.Length());
//DevMsg("cos_newvel = %.8f; cos_test1 = %.8f; cos_test2 = %.8f\n", cos_newvel, cos_test1, cos_test2);
if (cos_test1 < cos_newvel) {
if (cos_test2 < cos_test1) {
newvel = test_vel2;
resulting_yaw = test_yaw2;
cos_newvel = cos_test2;
} else {
newvel = test_vel1;
resulting_yaw = test_yaw1;
cos_newvel = cos_test1;
}
} else if (cos_test2 < cos_newvel) {
newvel = test_vel2;
resulting_yaw = test_yaw2;
cos_newvel = cos_test2;
}
} else {
//DevMsg("theta = %.08f, yaw = %.08f, vel_yaw = %.08f, speed = %.08f\n", theta, yaw, vel_yaw, player.Velocity.Length2D());
}
player.Velocity.AsVector2D() = newvel;
return resulting_yaw;
}
void Quaternion::setPow(const Quaternion& q, Real n)
{
Real mag = q.norm();
Real immag = q.imNorm();
Real immaginv;
if(immag == Zero) immaginv = Zero; //it's a real number, zero out the imaginary part
else immaginv = One / immag;
Real theta = Atan2(immag, q.w);
Real cntheta = Cos(n*theta);
Real sntheta = Sin(n*theta);
Real powm = Pow(mag, n);
w = powm * cntheta;
x = powm * sntheta * immaginv * q.x;
y = powm * sntheta * immaginv * q.y;
z = powm * sntheta * immaginv * q.z;
}
void AxisRotationFit(const std::vector<Vector3>& a,const std::vector<Vector3>& b,const Vector3& z,Real& theta)
{
Assert(!a.empty());
Assert(a.size()==b.size());
//min sum||R*a[i]-b[i]||^2
// = sum (R*a[i]-b[i]).(R*a[i]-b[i])
// = sum a[i].a[i] + b[i].b[i] - 2 b[i].R*a[i]
//differentiating, get
// 0 = sum b[i].R'*a[i] = sum b[i].[z]R*a[i]
//Let s=sin(theta) and c=cos(theta).
// R = cI + (1-c)zz' + s[z]
//so
// 0 = sum c*b[i]'[z]a[i] + (1-c)b[i]'[z]zz'a[i] + sb[i]'[z][z]a[i]
// = c*sum b[i]'[z]a[i] + s*b[i]'[z][z]a[i]
// [z] = [0 -z y]
// [z 0 -x]
// [-y x 0]
// [z]^2 = [-zz-yy xy xz ]
// [xy -xx-zz yz ]
// [xz yz -xx-yy]
//collecting terms by s and c,
//get s(sum -axbx-ayby) + c(sum axby-aybx) = 0
//solve using theta = atan(sum axby-aybx / sum -axbx-ayby)
//is it theta or theta+pi?
Matrix3 zcross,zcross2;
zcross.setCrossProduct(z);
zcross2.mul(zcross,zcross);
Real scoeff=0,ccoeff=0;
for(size_t i=0;i<a.size();i++)
scoeff += b[i].dot(zcross2*a[i]);
for(size_t i=0;i<b.size();i++)
ccoeff += b[i].dot(zcross*a[i]);
if(FuzzyZero(scoeff) && FuzzyZero(ccoeff))
theta=0;
else
theta = Atan2(-ccoeff,scoeff);
Real c=Cos(theta),s=Sin(theta);
if(c*scoeff-s*ccoeff > 0) {
theta += Pi;
}
}
void TransformCosSin_Sin(Real a,Real b,Real& c,Real& d)
{
//use sin(x+d) = sin(x)cos(d) + cos(x)sin(d)
//=> a=c*sin(d), b=c*cos(d)
//=> c^2 = a^2+b^2
if(a==0 && b==0) { c=d=0; }
else {
d = Atan2(a,b);
c = pythag(a,b);
}
Real x=0.5;
if(!FuzzyEquals(c*Sin(x+d),a*Cos(x)+b*Sin(x))) {
printf("Error in TransformCosSin\n");
printf("a: %f, b: %f\n",a,b);
printf("c: %f, d: %f\n",c,d);
printf("f(x): %f\n",a*Cos(x)+b*Sin(x));
printf("g(x): %f\n",c*Sin(x+d));
}
Assert(FuzzyEquals(c*Sin(x+d),a*Cos(x)+b*Sin(x)));
}
int CDanmakuToPlayerLine::Fire()
{
double angle = offset + Atan2( g_pPlayer->GetX() - pTama->x, g_pPlayer->GetY() - pTama->y);
for( int i = 0; i < n; ++i ) {
double nn = (double)(n-1) / 2.0;
double a;
if( nn != 0 )
a = angle + w * ( (double)i - nn) / nn;
else
a = angle;
for( int j = 0; j < m; ++j ) {
CTama *p = pTama->Copy();
if( m != 1 )
p->v = this->sv + j * ( this->ev - this->sv ) / ( this->m - 1 );
else
p->v = this->sv;
p->a = a;
g_lTama.Add( p );
}
}
return 1;
}
int CDanmakuSegmentLine::Fire()
{
double vx1 = v1 * Cos( a1 );
double vy1 = v1 * Sin( a1 );
double vx2 = v2 * Cos( a2 );
double vy2 = v2 * Sin( a2 );
double nn = n - 1;
for( int i = 0; i < n; ++i ) {
double vx = vx1 * i / nn + vx2 * ( nn - i ) / nn;
double vy = vy1 * i / nn + vy2 * ( nn - i ) / nn;
double vv = sqrt(pow2(vx) + pow2(vy));
double aa = Atan2(vx,vy);
for( int j = 0; j < m; ++j ) {
CTama *p = pTama->Copy();
if( m == 1 )
p->v = vv;
else
p->v = vv * j / (m-1) + vv * ev * (m-1-j) / (m-1);
p->a = aa;
g_lTama.Add( p );
}
}
return 1;
}
void curve4_div::recursive_bezier(scalar x1, scalar y1, scalar x2, scalar y2, scalar x3, scalar y3, scalar x4, scalar y4, unsigned int level)
{
if (level > curve_recursion_limit) {
return;
}
// Calculate all the mid-points of the line segments
//----------------------
scalar x12 = (x1 + x2) / 2;
scalar y12 = (y1 + y2) / 2;
scalar x23 = (x2 + x3) / 2;
scalar y23 = (y2 + y3) / 2;
scalar x34 = (x3 + x4) / 2;
scalar y34 = (y3 + y4) / 2;
scalar x123 = (x12 + x23) / 2;
scalar y123 = (y12 + y23) / 2;
scalar x234 = (x23 + x34) / 2;
scalar y234 = (y23 + y34) / 2;
scalar x1234 = (x123 + x234) / 2;
scalar y1234 = (y123 + y234) / 2;
// Try to approximate the full cubic curve by a single straight line
//------------------
scalar dx = x4-x1;
scalar dy = y4-y1;
scalar d2 = Fabs(((x2 - x4) * dy - (y2 - y4) * dx));
scalar d3 = Fabs(((x3 - x4) * dy - (y3 - y4) * dx));
scalar da1, da2, k;
switch((int(d2 > curve_collinearity_epsilon) << 1) + int(d3 > curve_collinearity_epsilon))
{
case 0:
// All collinear OR p1==p4
//----------------------
k = dx*dx + dy*dy;
if (k == 0) {
d2 = calc_sq_distance(x1, y1, x2, y2);
d3 = calc_sq_distance(x4, y4, x3, y3);
} else {
k = 1 / k;
da1 = x2 - x1;
da2 = y2 - y1;
d2 = k * (da1*dx + da2*dy);
da1 = x3 - x1;
da2 = y3 - y1;
d3 = k * (da1*dx + da2*dy);
if (d2 > 0 && d2 < 1 && d3 > 0 && d3 < 1) {
// Simple collinear case, 1---2---3---4
// We can leave just two endpoints
return;
}
if (d2 <= 0)
d2 = calc_sq_distance(x2, y2, x1, y1);
else if(d2 >= 1)
d2 = calc_sq_distance(x2, y2, x4, y4);
else
d2 = calc_sq_distance(x2, y2, x1 + d2*dx, y1 + d2*dy);
if (d3 <= 0)
d3 = calc_sq_distance(x3, y3, x1, y1);
else if(d3 >= 1)
d3 = calc_sq_distance(x3, y3, x4, y4);
else
d3 = calc_sq_distance(x3, y3, x1 + d3*dx, y1 + d3*dy);
}
if (d2 > d3) {
if (d2 < m_distance_tolerance_square) {
m_points.add(vertex_s(x2, y2));
return;
}
} else {
if (d3 < m_distance_tolerance_square) {
m_points.add(vertex_s(x3, y3));
return;
}
}
break;
case 1:
// p1,p2,p4 are collinear, p3 is significant
//----------------------
if (d3 * d3 <= m_distance_tolerance_square * (dx*dx + dy*dy)) {
if (m_angle_tolerance < curve_angle_tolerance_epsilon) {
m_points.add(vertex_s(x23, y23));
return;
}
// Angle Condition
//----------------------
da1 = Fabs(Atan2(y4 - y3, x4 - x3) - Atan2(y3 - y2, x3 - x2));
if (da1 >= PI)
da1 = _2PI - da1;
if (da1 < m_angle_tolerance) {
m_points.add(vertex_s(x2, y2));
m_points.add(vertex_s(x3, y3));
return;
}
if (m_cusp_limit != 0.0) {
if (da1 > m_cusp_limit) {
m_points.add(vertex_s(x3, y3));
return;
}
}
}
break;
case 2:
// p1,p3,p4 are collinear, p2 is significant
//----------------------
if (d2 * d2 <= m_distance_tolerance_square * (dx*dx + dy*dy)) {
if (m_angle_tolerance < curve_angle_tolerance_epsilon) {
m_points.add(vertex_s(x23, y23));
return;
}
// Angle Condition
//----------------------
da1 = Fabs(Atan2(y3 - y2, x3 - x2) - Atan2(y2 - y1, x2 - x1));
if (da1 >= PI)
da1 = _2PI - da1;
if (da1 < m_angle_tolerance) {
m_points.add(vertex_s(x2, y2));
m_points.add(vertex_s(x3, y3));
return;
}
if (m_cusp_limit != 0.0) {
if (da1 > m_cusp_limit) {
m_points.add(vertex_s(x2, y2));
return;
}
}
}
break;
case 3:
// Regular case
//-----------------
if ((d2 + d3)*(d2 + d3) <= m_distance_tolerance_square * (dx*dx + dy*dy)) {
// If the curvature doesn't exceed the distance_tolerance value
// we tend to finish subdivisions.
//----------------------
if (m_angle_tolerance < curve_angle_tolerance_epsilon) {
m_points.add(vertex_s(x23, y23));
return;
}
// Angle & Cusp Condition
//----------------------
k = Atan2(y3 - y2, x3 - x2);
da1 = Fabs(k - Atan2(y2 - y1, x2 - x1));
da2 = Fabs(Atan2(y4 - y3, x4 - x3) - k);
if (da1 >= PI)
da1 = _2PI - da1;
if (da2 >= PI)
da2 = _2PI - da2;
if (da1 + da2 < m_angle_tolerance) {
// Finally we can stop the recursion
//----------------------
m_points.add(vertex_s(x23, y23));
return;
}
if (m_cusp_limit != 0.0) {
if (da1 > m_cusp_limit) {
m_points.add(vertex_s(x2, y2));
return;
}
if (da2 > m_cusp_limit) {
m_points.add(vertex_s(x3, y3));
return;
}
}
}
break;
}
// Continue subdivision
//----------------------
recursive_bezier(x1, y1, x12, y12, x123, y123, x1234, y1234, level + 1);
recursive_bezier(x1234, y1234, x234, y234, x34, y34, x4, y4, level + 1);
}
inline Base<F> Arg( const F& alpha )
{ return Atan2( ImagPart(alpha), RealPart(alpha) ); }
static int calculate(int x, int y, int d)
{
scalar a = Atan2(INT_TO_SCALAR(y), INT_TO_SCALAR(x));
if (a < 0) a = _2PI + a;
return iround(a * INT_TO_SCALAR(d) * _1div2PI);
}
static int calculate(int x, int y, int d)
{
return uround(Fabs(Atan2(INT_TO_SCALAR(y), INT_TO_SCALAR(x))) * INT_TO_SCALAR(d) * _1divPI);
}
//------------------------------------------------------------------------
void curve3_div::recursive_bezier(scalar x1, scalar y1, scalar x2, scalar y2, scalar x3, scalar y3, unsigned int level)
{
if (level > curve_recursion_limit) {
return;
}
// Calculate all the mid-points of the line segments
//----------------------
scalar x12 = (x1 + x2) / 2;
scalar y12 = (y1 + y2) / 2;
scalar x23 = (x2 + x3) / 2;
scalar y23 = (y2 + y3) / 2;
scalar x123 = (x12 + x23) / 2;
scalar y123 = (y12 + y23) / 2;
scalar dx = x3-x1;
scalar dy = y3-y1;
scalar d = Fabs(((x2 - x3) * dy - (y2 - y3) * dx));
scalar da;
if (d > curve_collinearity_epsilon) {
// Regular case
//-----------------
if (d * d <= m_distance_tolerance_square * (dx*dx + dy*dy)) {
// If the curvature doesn't exceed the distance_tolerance value
// we tend to finish subdivisions.
//----------------------
if (m_angle_tolerance < curve_angle_tolerance_epsilon) {
m_points.add(vertex_s(x123, y123));
return;
}
// Angle & Cusp Condition
//----------------------
da = Fabs(Atan2(y3 - y2, x3 - x2) - Atan2(y2 - y1, x2 - x1));
if (da >= PI)
da = _2PI - da;
if (da < m_angle_tolerance) {
// Finally we can stop the recursion
//----------------------
m_points.add(vertex_s(x123, y123));
return;
}
}
} else {
// Collinear case
//------------------
da = dx*dx + dy*dy;
if (da == 0) {
d = calc_sq_distance(x1, y1, x2, y2);
} else {
d = ((x2 - x1)*dx + (y2 - y1)*dy) / da;
if (d > 0 && d < 1) {
// Simple collinear case, 1---2---3
// We can leave just two endpoints
return;
}
if (d <= 0)
d = calc_sq_distance(x2, y2, x1, y1);
else if (d >= 1)
d = calc_sq_distance(x2, y2, x3, y3);
else
d = calc_sq_distance(x2, y2, x1 + d*dx, y1 + d*dy);
}
if (d < m_distance_tolerance_square) {
m_points.add(vertex_s(x2, y2));
return;
}
}
// Continue subdivision
//----------------------
recursive_bezier(x1, y1, x12, y12, x123, y123, level + 1);
recursive_bezier(x123, y123, x23, y23, x3, y3, level + 1);
}
