/**
* Der Winkel zur Z-Achse bei Rotation um Y-Achse
* @return der Winkel, um den der Vektor um die Y-Achse rotiert werden müsste, um zur Z-Achse koliniar zu sein.
*/
GLdouble SGLVektor::Ywink()
{
GLdouble teiler=sqrt(SGLV_X*SGLV_X+SGLV_Z*SGLV_Z);
if(SGLV_X>=0)
{
if(SGLV_Z>=0)return ACOS(SGLV_X/teiler);
else return 360-ACOS(SGLV_X/teiler);
}
else
{
if(SGLV_Z>=0)return 180-ACOS(SGLV_X/teiler);
else return 180+ACOS(SGLV_X/teiler);
}
}
/**
* Der Winkel zu Z-Achse bei Rotation um X-Achse
* @return der Winkel, um den der Vektor um die X-Achse rotiert werden müsste, um zur Z-Achse koliniar zu sein.
*/
GLdouble SGLVektor::Xwink()
{
GLdouble teiler=sqrt(SGLV_Z*SGLV_Z+SGLV_Y*SGLV_Y);
if(SGLV_Z>=0)
{
if(SGLV_Y>=0)return ACOS(SGLV_Z/teiler);
else return 360-ACOS(SGLV_Z/teiler);
}
else
{
if(SGLV_Y>=0)return 180-ACOS(SGLV_Z/teiler);
else return 180+ACOS(SGLV_Z/teiler);
}
return 0;
}
bool Line::isParallel(const Line& l,REAL biasAngle) const
{
REAL a=ACOS(FABS((p[0]-p[1]).normal()*(l[0]-p[1]).normal()));
if(a<=biasAngle)
return true;
return false;
}
Int32 Cubic( Real x3, Real x2, Real x, Real k, Real roots[3] )
{
if( IsZero( x3 ) )
return Quadratic( x2, x, k, roots );
Real a, b, c, d, a2, p, q, p3, s;
// Normalize
a = x2 / x3;
b = x / x3;
c = k / x3;
// Reduction to a depressed cubic
a2 = a * a;
p = 1.0f / 3.0f * ( -1.0f / 3.0f * a2 + b );
q = 1.0f / 2.0f * ( 2.0f / 27.0f * a * a2 - 1.0f / 3.0f * a * b + c );
s = 1.0f / 3.0f * a;
// Cardano's method
p3 = p * p * p;
d = q * q + p3;
if( IsZero( d ) )
{
if( IsZero( q ) )
{
roots[0] = -s;
return 1;
}
else
{
Real u = CubeRoot( -q );
roots[0] = 2.0f * u - s;
roots[1] = -u - s;
return 2;
}
}
if( d < 0.0f )
{
Real phi = 1.0f / 3.0f * ACOS( -q / SQRT( -p3 ) );
Real t = 2.0f * SQRT( -p );
roots[0] = t * COS( phi ) - s;
roots[1] = -t * COS( phi + PI / 3.0f ) - s;
roots[2] = -t * COS( phi - PI / 3.0f ) - s;
return 3;
}
Real u = CubeRoot( SQRT( d ) + ABS( q ) );
roots[0] = ( q > 0.0f ? - u + p / u : u - p / u ) - s;
return 1;
}
void AstroClock::convert(uint32_t timestamp, float ra, float dec, float* alt,
float* az) {
float lstd = getLSTd(convertJ2000(timestamp));
float ha = lstd - ra;
while (ha < 0)
ha += 360.0;
float sinAlt = SIN(dec) * SIN(lat) + COS(dec) * COS(lat) * COS(ha);
*alt = ASIN(sinAlt);
float cosA = //
(SIN(dec) - SIN(*alt) * SIN(lat)) //
/ (COS(*alt) * COS(lat));
float a = ACOS(cosA);
if (SIN(ha) < 0)
*az = a;
else
*az = 360.0 - a;
}
DBstatus GE706(
DBAny *pstr1,
DBAny *pstr2,
short *noint,
DBfloat u1[],
DBfloat u2[])
/* Compute intersect between line and arc/circle in 2D.
*
* In: pstr1 => First entity
* pstr2 => Second entity
* noint => Requested solution +/-
*
* Out: *noint => Number of solutions
* *u1 => Parametric values related to the line
* *u2 => Parameteric values related to the arc
*
* (C)microform ab 5/8/85 R. Svedin efter IVAR
*
* 12/8/85 Anpassning till v3, J.Kjellander
* 3/1/86 Tangering, J. Kjellander
* 15/1/86 Bug lodrät linje, J. Kjellander
* 13/4/86 Bytt TOL1 mot TOL2, J. Kjellander
* 19/4/86 Horisontell linje, J. Kjellander
* 26/11/89 Neg intnr, J. Kjellander
* 1999-05-10 Rewritten, J.Kjellander
*
******************************************************************!*/
{
DBLine *linpek;
DBArc *arcpek;
DBfloat k,k2,p,q,dx,dy;
DBfloat x1,y1,x2,y2,tt,tmp;
DBfloat t1[2],t2[2];
short i;
/*
***Which is which ?
*/
if ( pstr1->hed_un.type == LINTYP )
{
linpek = (GMLIN *)pstr1;
arcpek = (GMARC *)pstr2;
}
else
{
linpek = (GMLIN *)pstr2;
arcpek = (GMARC *)pstr1;
}
/*
***Some init.
*/
dx = linpek->crd2_l.x_gm - linpek->crd1_l.x_gm;
dy = linpek->crd2_l.y_gm - linpek->crd1_l.y_gm;
/*
***Vertical, horisontal or sloping line ?
*
***Horisontal.
*/
if ( ABS(dx) > 1000.0*ABS(dy) )
{
if ( ABS(linpek->crd1_l.y_gm-arcpek->y_a)-arcpek->r_a > TOL2 )
{
*noint = 0;
return(0);
}
y1 = y2 = linpek->crd1_l.y_gm;
if ( arcpek->r_a > ABS(linpek->crd1_l.y_gm-arcpek->y_a) )
{
x1 = arcpek->x_a + arcpek->r_a*COS(ASIN((linpek->
crd1_l.y_gm-arcpek->y_a)/arcpek->r_a));
x2 = 2.0*arcpek->x_a - x1;
}
else
{
x1 = x2 = arcpek->x_a;
}
t1[0] = 1.0 + (x1 - linpek->crd1_l.x_gm) / dx;
t1[1] = 1.0 + (x2 - linpek->crd1_l.x_gm) / dx;
}
/*
***Vertical.
*/
else if ( ABS(dy) > 1000.0*ABS(dx) )
{
if ( ABS(linpek->crd1_l.x_gm-arcpek->x_a)-arcpek->r_a > TOL2 )
{
*noint = 0;
return(0);
}
x1 = x2 = linpek->crd1_l.x_gm;
if ( arcpek->r_a > ABS(linpek->crd1_l.x_gm-arcpek->x_a) )
{
y1 = arcpek->y_a + arcpek->r_a*SIN(ACOS((linpek->
crd1_l.x_gm-arcpek->x_a)/arcpek->r_a));
y2 = 2.0*arcpek->y_a - y1;
}
else
{
y1 = y2 = arcpek->y_a;
}
t1[0] = 1.0 + (y1 - linpek->crd1_l.y_gm) / dy;
t1[1] = 1.0 + (y2 - linpek->crd1_l.y_gm) / dy;
}
/*
***Sloping.
*/
else
{
k = dy/dx;
k2 = k*k;
p = (arcpek->x_a + k2 * linpek->crd1_l.x_gm - k *
(linpek->crd1_l.y_gm - arcpek->y_a)) / (1.0 + k2);
q = (arcpek->x_a * arcpek->x_a + k2 * linpek->crd1_l.x_gm *
linpek->crd1_l.x_gm + 2.0 * k * linpek->crd1_l.x_gm *
(arcpek->y_a - linpek->crd1_l.y_gm) + (linpek->crd1_l.y_gm -
arcpek->y_a) * (linpek->crd1_l.y_gm - arcpek->y_a) -
arcpek->r_a * arcpek->r_a) / (1.0 + k2);
/*
***Do they intersect ?
*/
if ( (tt=p*p-q) < 0.0 && tt > -TOL2 ) tt = 0.0;
if ( tt >= 0.0 )
{
/*
***Yes, analytical solution.
*/
x1 = p+SQRT(tt);
y1 = k*(x1-linpek->crd1_l.x_gm) + linpek->crd1_l.y_gm;
x2 = p-SQRT(tt);
y2 = k*(x2-linpek->crd1_l.x_gm) + linpek->crd1_l.y_gm;
}
/*
***No intersect.
*/
else
{
*noint = 0;
return(0);
}
/*
***Compute line parametric values.
*/
t1[0] = 1.0 + (x1 - linpek->crd1_l.x_gm) / dx;
t1[1] = 1.0 + (x2 - linpek->crd1_l.x_gm) / dx;
}
/*
***Compute arc parametric values.
*/
dx = x1 - arcpek->x_a;
dy = y1 - arcpek->y_a;
GE315(arcpek,dx,dy,&t2[0]);
dx = x2 - arcpek->x_a;
dy = y2 - arcpek->y_a;
GE315(arcpek,dx,dy,&t2[1]);
/*
***If *noint > 0, remove intersects outside actual length of entity.
*/
if ( *noint > 0 )
{
*noint = 2;
if ( t1[0] < 1.0-TOL4 || t1[0] > 2.0+TOL4 ||
t2[0] < 1.0-TOL4 || t2[0] > 2.0+TOL4 )
{
t1[0] = t1[1];
t2[0] = t2[1];
*noint = 1;
}
if ( t1[1] < 1.0-TOL4 || t1[1] > 2.0+TOL4 ||
t2[1] < 1.0-TOL4 || t2[1] > 2.0+TOL4 )
{
*noint = *noint - 1;
}
}
else *noint = 2;
/*
***Copy remaining solutions to u1 and u2.
*/
for ( i=0; i<*noint; ++i )
{
if ( pstr1->hed_un.type == LINTYP )
{
u1[i] = t1[i];
u2[i] = t2[i];
}
else
{
u1[i] = t2[i];
u2[i] = t1[i];
}
}
/*
***If more than one solution left sort in increasing order
***with respect of the first entity.
*/
if ( *noint > 1 && u1[0] > u1[1] )
{
tmp = u1[0];
u1[0] = u1[1];
u1[1] = tmp;
tmp = u2[0];
u2[0] = u2[1];
u2[1] = tmp;
}
return(0);
}
/* Sun rise-set calculation algorithm.
* Algorithm description: http://williams.best.vwh.net/sunrise_sunset_algorithm.htm
*
* Almanac for Computers, 1990
* published by Nautical Almanac Office
* United States Naval Observatory
* Washington, DC 20392
*
* Parameters:
* yd - day of the year (1..365)
* latitude, longitude - Sample: 32.27, 34.85 for Netania israel
* riseset - ESUNRISE or ESUNSET
*
* Returns: UTC hour of the event (a real number).
*/
double sunriseset( int yd, double latitude, double longitude, ERiseSet riseset )
{
// 96 degrees - Calculate Civil twilight time. Used as indication if
// it is (usually) bright enough for outdoor activities without additional lighting.
// 90 degrees 5' - Calculate true Sunrise/Sunset time. Used to check if
// the Sun itself is visible above the horizont in ideal conditions.
const double zenith = 96;
double sinDec, cosDec, cosH;
double H, T, UT;
// Rise / Set
int op = (riseset==ESUNRISE ? 1:-1);
// Convert the longitude to hour value and calculate an approximate time
double lngHour = longitude / 15;
// if rising time is desired:
double t = yd + ((12 - (6*op) - lngHour) / 24);
// Calculate the Sun's mean anomaly
double M = (0.9856 * t) - 3.289;
// Calculate the Sun's true longitude
double L = M + (1.916 * SIN(M)) + (0.020 * SIN(2 * M)) + 282.634;
// Calculate the Sun's right ascension
double RA = ATAN(0.91764 * TAN(L));
// Right ascension value needs to be in the same quadrant as L
double Lquadrant = (floor( L/90)) * 90;
double RAquadrant = (floor(RA/90)) * 90;
RA = RA + (Lquadrant - RAquadrant);
// Right ascension value needs to be converted into hours
RA = RA / 15;
// Calculate the Sun's declination
sinDec = 0.39782 * SIN(L);
cosDec = COS(ASIN(sinDec));
// Calculate the Sun's local hour angle
cosH = (COS(zenith) - (sinDec * SIN(latitude))) / (cosDec * COS(latitude));
if( cosH < -1 || cosH > 1 )
// The sun never rises or sets on this location (on the specified date)
return -1;
// Finish calculating H and convert into hours
H = 180 + (180 - ACOS(cosH))*op;
H = H / 15;
// Calculate local mean time of rising/setting
T = H + RA - (0.06571 * t) - 6.622;
// Adjust back to UTC
UT = T - lngHour;
// UT potentially needs to be adjusted into the range [0,24) by adding/subtracting 24
if (UT < 0) UT += 24;
if (UT >= 24) UT -= 24;
return UT;
}
/*.BE*/
int spltrans /* transformiert-parametr. kub. Polynomspline .......*/
/*.BA*/
/*.IX{spltrans}*/
/*.BE*/
(
int m, /* Anzahl der Stuetzpunkte ..............*/
REAL x[], /* Stuetzstellen ........................*/
REAL y[], /* Stuetzwerte ..........................*/
int mv, /* Art der Koordinatenverschiebung ......*/
REAL px[], /* Koordinaten des ......................*/
REAL py[], /* Verschiebepunktes P ..................*/
REAL a[], /* Splinekoeff. von (phi-phin[i])^0 .....*/
REAL b[], /* Splinekoeff. von (phi-phin[i]) .......*/
REAL c[], /* Splinekoeff. von (phi-phin[i])^2 .....*/
REAL d[], /* Splinekoeff. von (phi-phin[i])^3 .....*/
REAL phin[], /* Winkelkoordinaten der Stuetzpunkte ...*/
REAL *phid /* Drehwinkel des Koordinatensystems ....*/
) /* Fehlercode ...........................*/
/*.BA*/
/***********************************************************************
* die Koeffizienten einer transformiert-parametrischen interpolieren- *
* den kubischen Splinefunktion fuer eine geschlossene, ueberall glatte *
* Kurve berechnen. *
.BE*)
* Eine transformiert-parametrische kubische Splinefunktion ist eine *
* periodische kubische Splinefunktion wie in der Funktion spline(), *
* jedoch in Polarkoordinatendarstellung. Dies ermoeglicht in vielen *
* Faellen die Interpolation von Daten, deren Stuetzstellen nicht mono- *
* ton steigend angeordnet sind, ohne echte parametrische Splines (wie *
* in der Funktion parspl()) berechnen zu muessen. *
* Hierzu transformiert die Funktion die eingegebenen Punkte zunaechst *
* auf Polarkoordinaten (phin[i],a[i]), wobei phin[i] der Winkel und *
* a[i] die Laenge des Ortsvektors von (x[i],y[i]) ist, i=0(1)m-1. Dies *
* muss so moeglich sein, dass die Winkelwerte phin[i] streng monoton *
* steigen, andernfalls ist das transformiert-parametrische Verfahren *
* nicht anwendbar. Dann wird eine periodische kubische Splinefunktion *
* mit den Winkeln phin[i] als Stuetzstellen berechnet, die die Vektor- *
* laengen a[i] interpoliert. Um die Monotonie der phin[i] zu errei- *
* chen, kann es notwendig sein, den Koordinatenursprung auf einen *
* Punkt P = (px, py) zu verschieben und das Koordinatensystem um ei- *
* nen Winkel phid zu drehen. (px, py) muss so in der durch die *
* (x[i], y[i]) beschriebenen Flaeche liegen, dass jeder von P ausge- *
* hende Polarstrahl die Randkurve der Flaeche nur einmal schneidet. *
* P kann sowohl vom Benutzer vorgegeben als auch von der Funktion be- *
* rechnet werden. Der hier berechnete Wert ist allerdings nur als Vor- *
* schlagswert aufzufassen, der in unguenstigen Faellen nicht immer die *
* Bedingungen erfuellt. Ausserdem muessen die (x[i],y[i]) in der Rei- *
* henfolge angeordnet sein, die sich ergibt, wenn man die Randkurve *
* der Flaeche, beginnend bei i = 1, im mathematisch positiven Sinn *
* durchlaeuft. Da die Kurve geschlossen ist, muss x[m-1] = x[0] und *
* y[m-1] = y[0] sein. *
* *
* Eingabeparameter: *
* ================= *
* m: Anzahl der Stuetzstellen (mindestens 3) *
* x: [0..m-1]-Vektor mit den Stuetzstellen *
* y: [0..m-1]-Vektor mit den zu interpolierenden Stuetzwerten *
* mv: Marke fuer die Verschiebung des Koordinatenursprungs. *
* mv > 0: Der Benutzer gibt die Koordinaten px, py vor. *
* mv = 0: keine Verschiebung (d.h. px = py = 0) *
* mv < 0: px und py werden hier berechnet. *
* Es wird gesetzt: *
* px = (xmax + xmin) / 2 *
* py = (ymax + ymin) / 2 *
* mit xmax = max(x[i]), xmin = min(x[i]), *
* ymax = max(y[i]), ymin = min(y[i]), i=0(1)m-1. *
* Zur Beachtung: Hierdurch ist nicht notwendigerweise *
* sichergestellt, dass P die oben genannten Bedingungen *
* erfuellt. Falls die Funktion mit dem Fehlercode -3 *
* endet, muss P vom Benutzer vorgegeben werden. *
* *
* px: \ fuer mv > 0: vorgegebene Koordinaten des Punktes P *
* py: / *
* *
* Ausgabeparameter: *
* ================= *
* a: \ [0..m-1]-Vektoren mit Splinekoeffizienten in der Darstellung *
* b: \ S(phi) = a[i] + b[i] * (phi - phin[i]) *
* c: / + c[i] * (phi - phin[i]) ^ 2 *
* d: / + d[i] * (phi - phin[i]) ^ 3 *
* fuer phin[i] <= phi <= phin[i+1], i=0(1)m-2 . *
* Die a[i] sind die Vektorlaengen der (x[i],y[i]) in der Polar- *
* koordinatendarstellung. b, c und d werden auch noch fuer *
* Zwischenergebnisse missbraucht. *
* phin: [0..m-1]-Vektor mit den Winkelkoordinaten der (x[i],y[i]) in *
* der Polarkoordinatendarstellung. *
* Es ist phin[0] = 0, *
* phin[i] = arctan((y[i] - py) / (x[i] - px)) - phid, *
* i=1(1)m-2 *
* phin[m-1] = 2 * Pi *
* *
* px: \ Koordinaten des Verschiebungspunktes P *
* py: / *
* phid: Winkel, um den das Koordinatensystem eventuell gedreht wurde. *
* Es ist phid = arctan(y[0] / x[0]). *
* *
* Funktionswert: *
* ============== *
* 0: kein Fehler *
* -1: m < 3 *
* -3: Die phin[i] sind nicht streng monoton steigend. *
* -4: x[m-1] != x[0] oder y[m-1] != y[0] *
* >0: Fehler in spline() *
* *
* benutzte globale Namen: *
* ======================= *
* spline, REAL, PI, sqr, SQRT, ACOS, ZERO, TWO *
.BA*)
***********************************************************************/
/*.BE*/
{
REAL xmin, /* Minimum der x[i] */
xmax, /* Maximum der x[i] */
ymin, /* Minimum der y[i] */
ymax, /* Maximum der y[i] */
sa, /* sin(-phid) */
ca; /* cos(-phid) */
int n, /* m - 1, Index der letzten Stuetzstelle */
i; /* Laufvariable */
n = m - 1;
/* ---------------- die Vorbedingungen ueberpruefen --------------- */
if (n < 2)
return -1;
if (x[0] != x[n] || y[0] != y[n])
return -4;
/* ---------------- die Koordinaten transformieren ---------------- */
if (mv == 0) /* das Koordinatensystem nicht verschieben? */
{
*px = *py = ZERO;
for (i = 0; i <= n; i++)
b[i] = x[i],
c[i] = y[i];
}
else /* den Koordinatenursprung nach (px, py) verschieben? */
{
if (mv < 0) /* Sollen py und py berechnet werden? */
{
xmax = x[0];
xmin = x[0];
ymax = y[0];
ymin = y[0];
for (i = 1; i <= n; i++)
{
if (x[i] > xmax)
xmax = x[i];
if (x[i] < xmin)
xmin = x[i];
if (y[i] > ymax)
ymax = y[i];
if (y[i] < ymin)
ymin = y[i];
}
*px = (xmax + xmin) / TWO;
*py = (ymax + ymin) / TWO;
}
for (i = 0; i <= n; i++) /* die verschoben Punkte (x[i],y[i]) */
b[i] = x[i] - *px, /* in (b[i],c[i]) aufgewahren */
c[i] = y[i] - *py;
}
/* ---- die transformierten Stuetzstellen berechnen: ---- */
/* ---- 1. die a[i] berechnen. Abbruch, wenn a[i] = 0, d. h. ---- */
/* ---- wenn (px, py) mit einer Stuetzstelle zusammenfaellt ---- */
for (i = 0; i <= n; i++)
{
a[i] = SQRT(sqr(b[i]) + sqr(c[i]));
if (a[i] == ZERO)
return -3;
}
/*------------------------------------------------------------------*/
/* 2. die um alpha gedrehten Koordinaten X1, Y1 berechnen */
/* nach den Gleichungen: */
/* */
/* (X1) ( cos(alpha) -sin(alpha) ) (X) */
/* ( ) = ( ) ( ) */
/* (Y1) ( sin(alpha) cos(alpha) ) (Y) */
/* */
/* mit alpha = -phid */
/*------------------------------------------------------------------*/
*phid = ACOS(b[0] / a[0]);
if (c[0] < ZERO)
*phid = TWO * PI - *phid;
ca = b[0] / a[0];
sa = -c[0] / a[0];
for (i = 0; i <= n; i++) /* die gedrehten Koordinaten */
d[i] = b[i] * ca - c[i] * sa, /* (b[i],c[i]) in */
c[i] = b[i] * sa + c[i] * ca; /* (d[i],c[i] ablegen */
/* ------ die Winkelkoordinaten phin[i] berechnen. Abbruch, ------ */
/* ------ wenn die Winkel nicht streng monoton steigend sind ------ */
phin[0] = ZERO;
for (i = 1; i < n; i++)
{
phin[i] = ACOS(d[i] / a[i]);
if (c[i] < ZERO)
phin[i] = TWO * PI - phin[i];
if (phin[i] <= phin[i - 1])
return -3;
}
phin[n] = TWO * PI;
/* --------------- die Splinekoeffizienten berechnen -------------- */
return spline(n + 1, phin, a, 4, ZERO, ZERO, 0, b, c, d);
}
REAL Vec2::angle(const Vec2& other) const
{
return ACOS(((*this)*other)/(length()*other.length()));
}
REAL Vec2::angle() const
{
Vec2 n=this->normal();
REAL angle=ACOS(n.x);
return SETSIGN(angle,SIGNBIT(n.y));
}
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);
}
}
// calc angle between 0 to pi
m_real vectorn::angle(vectorn const& b) const
{
vectorn const& a=*this;
return (m_real)(ACOS(a.cosTheta(b)));
}
