double Bridge:: find_zeta_max(const double theta)
{
// a good value
double zeta_lo = 0.0;
if( find_alpha(theta, zeta_lo, NULL) < 0 )
{
throw exception("Cannot find alpha for zeta=0");
}
// a bad balue
double zeta_up = 1.0;
while(find_alpha(theta,zeta_up,NULL)>=0)
{
zeta_lo = zeta_up;
zeta_up += 1.0;
}
std::cerr << "Looking up between " << zeta_lo << " and " << zeta_up << std::endl;
const double zeta_ftol = numeric<double>::sqrt_ftol;
while( Fabs(zeta_up-zeta_lo) >= zeta_ftol * ( Fabs(zeta_lo) + Fabs(zeta_up) ) )
{
const double zeta_mid = 0.5*(zeta_lo+zeta_up);
if(find_alpha(theta,zeta_mid,NULL) < 0 )
{
zeta_up = zeta_mid;
}
else
{
zeta_lo = zeta_mid;
}
}
// highest good value
return zeta_lo;
}
void Bubble:: map_peanut( const Vertex ¢er, Real radius, Real alpha )
{
assert(lambda>0);
empty();
alpha = clamp<Real>(0,Fabs(alpha),1);
const Real b = Fabs(radius);
const Real a = alpha * b;
const Real a2 = a*a;
const Real b2 = b*b;
const Real a4 = a2*a2;
const Real b4 = b2*b2;
const Real theta_max = 2 * atan( lambda/(radius+radius) );
const size_t nmin = 2*max_of<size_t>(3,size_t( ceil( numeric<Real>::two_pi/theta_max) ));
const Real dtheta = numeric<Real>::two_pi / nmin;
fprintf( stderr, "peanut @(%g,%g), b=%g, a=%g\n", center.x, center.y, b, a );
for( size_t i=0; i < nmin; ++i )
{
const Real theta = i * dtheta;;
const Real t2 = theta+theta;
Tracer *p = append();
const Real S = Sin(t2);
const Real c1 = max_of<Real>(0,b4 - a4 * S*S );
const Real C = max_of<Real>(0,a2 * Cos( t2 ) + Sqrt(c1));
const Real rho = Sqrt(C);
p->vertex.x = center.x + rho * Cos( theta );
p->vertex.y = center.y + rho * Sin( theta );
}
raw_initialize();
}
void gfx_gradient_adapter::init_radial(int spread, scalar x1, scalar y1, scalar radius1,
scalar x2, scalar y2, scalar radius2)
{
if (!m_wrapper) {
if ((x1 == x2) && (y1 == y2)) {
switch (spread) {
case SPREAD_PAD:
m_wrapper = new gfx_gradient<gradient_radial,
gradient_pad_adaptor<gradient_radial> >;
break;
case SPREAD_REPEAT:
m_wrapper = new gfx_gradient<gradient_radial,
gradient_repeat_adaptor<gradient_radial> >;
break;
case SPREAD_REFLECT:
m_wrapper = new gfx_gradient<gradient_radial,
gradient_reflect_adaptor<gradient_radial> >;
break;
}
} else {
switch (spread) {
case SPREAD_PAD:
m_wrapper = new gfx_gradient<gradient_radial_focus,
gradient_pad_adaptor<gradient_radial_focus> >;
break;
case SPREAD_REPEAT:
m_wrapper = new gfx_gradient<gradient_radial_focus,
gradient_repeat_adaptor<gradient_radial_focus> >;
break;
case SPREAD_REFLECT:
m_wrapper = new gfx_gradient<gradient_radial_focus,
gradient_reflect_adaptor<gradient_radial_focus> >;
break;
}
}
if (!m_wrapper)
return;
scalar len = Fabs(radius2);
scalar fx = x1 - x2;
scalar fy = y1 - y2;
m_wrapper->init(len, fx, fy);
if (!len)
len = FLT_TO_SCALAR(2.0f); // len can not be zero
gfx_trans_affine mtx;
mtx.translate(x1 - fx, y1 - fy);
m_start = Fabs(radius1);
m_length = len;
m_matrix = mtx;
}
}
//==============================================================================
//! simultaneous tridiagonal system
//==============================================================================
static inline
void vertex_tridiag(const array<Real> &a,
const array<Real> &b,
const array<Real> &c,
const array<Vertex> &r,
array<Vertex> &u,
const size_t n)
{
const Vertex org(0,0);
assert(n<=a.size());
assert(n<=b.size());
assert(n<=c.size());
assert(n<=r.size());
assert(n<=u.size());
assert(Fabs(b[1])>0);
Vertex bet;
vector<Vertex> gam(n,org);
u[1]=r[1]/(bet.x=bet.y=b[1]);
for(size_t j=2;j<=n;j++)
{
Vertex &gam_j = gam[j];
const Real c_jm = c[j-1];
gam_j.x = c_jm/bet.x;
gam_j.y = c_jm/bet.y;
const Real b_j = b[j];
const Real a_j = a[j];
bet.x = b_j - a_j * gam_j.x;
bet.y = b_j - a_j * gam_j.y;
if ( Fabs(bet.x) <= 0 || Fabs(bet.y) <= 0 )
throw exception("Error 2 in tridag");
const Vertex &r_j = r[j];
Vertex &u_j = u[j];
const Vertex &u_jm = u[j-1];
u_j.x = (r_j.x - a_j * u_jm.x) / bet.x;
u_j.y = (r_j.y - a_j * u_jm.y) / bet.y;
}
for(size_t j=(n-1);j>=1;j--)
{
Vertex &u_j = u[j];
const Vertex &u_jp = u[j+1];
const Vertex &gam_jp = gam[j+1];
u_j.x -= gam_jp.x * u_jp.x;
u_j.y -= gam_jp.y * u_jp.y;
}
}
FLOAT F_mul_F(FLOAT aa, FLOAT bb) {
uint32_t a, b, sgn;
unsigned long long res;
FLOAT ret;
a = Fabs(aa);
b = Fabs(bb);
sgn = (a != aa) ^ (b != bb);
res = (uint64_t)a * (uint64_t)b;
res >>= 16;
res &= 0x7FFFFFFF;
ret = res;
return sgn ? -ret : ret;
}
void Segmenter:: process_spot( const Spot *spot, const Real half )
{
const Tracer *tracer = spot->handle;
const Vertex vertex = tracer->vertex;
assert( vertex.x >= X[X.lower]);
assert( vertex.x < X[X.upper]);
assert( vertex.y >= Y[Y.lower]);
assert( vertex.y < Y[Y.upper]);
//--------------------------------------------------------------------------
// locate the tracer
//--------------------------------------------------------------------------
locate_vertex(vertex, spot->klo, spot->kup);
//--------------------------------------------------------------------------
// compute the absolute next coordinate
//--------------------------------------------------------------------------
Vertex target = vertex + tracer->edge;
//--------------------------------------------------------------------------
// find junctions
//--------------------------------------------------------------------------
compute_junctions(spot, vertex, target, tracer->next);
const Tracer *prec = tracer->prev;
if( ! prec->is_spot || Fabs(prec->vertex.y - vertex.y) >= half )
{
target = vertex - prec->edge;
compute_junctions(spot, vertex, target, prec);
}
}
/* Return the unit which yields the number with the least figures. */
const AG_Unit *
AG_BestUnit(const AG_Unit ugroup[], double n)
{
const AG_Unit *unit, *bestunit = NULL;
double smallest = HUGE_VAL;
double diff;
if (n == 0) {
goto defunit;
}
for (unit = &ugroup[0]; unit->key != NULL; unit++) {
if (n/unit->divider >= 1.0) {
diff = Fabs(n-unit->divider);
if (diff < smallest) {
smallest = diff;
bestunit = unit;
}
}
}
if (bestunit == NULL) {
goto defunit;
}
return (bestunit);
defunit:
for (unit = &ugroup[0]; unit->key != NULL; unit++) {
if (unit->divider == 1.0)
break;
}
return (unit);
}
int main() {
FLOAT a = computeT(10, f2F(-1.0), f2F(1.0), f);
FLOAT ans = f2F(0.551222);
nemu_assert(Fabs(a - ans) < f2F(1e-4));
HIT_GOOD_TRAP;
return 0;
}
int calculate(int x, int y, int) const
{
scalar dx = INT_TO_SCALAR(x - m_fx);
scalar dy = INT_TO_SCALAR(y - m_fy);
scalar d2 = dx * m_fy - dy * m_fx;
scalar d3 = m_r2 * (dx * dx + dy * dy) - d2 * d2;
return iround((dx * m_fx + dy * m_fy + Sqrt(Fabs(d3))) * m_mul);
}
FLOAT sqrt(FLOAT x) {
FLOAT dt, t = int2F(2);
do {
dt = F_div_int((F_div_F(x, t) - t), 2);
t += dt;
} while(Fabs(dt) > f2F(1e-4));
return t;
}
static inline Lisp_Object
_ase_metric_p_1dim_p(Lisp_Object a, Lisp_Object b, unsigned int p)
{
Lisp_Object tmp = ent_binop(ASE_BINARY_OP_DIFF, a, b);
Lisp_Object result = ent_binop(ASE_BINARY_OP_POW, tmp, make_int(p));
if ((p & 1) == 0)
return result;
else
return Fabs(result);
}
/* Compute the intersection of two circles. */
M_GeomSet2
M_IntersectCircleCircle2(M_Circle2 C1, M_Circle2 C2)
{
M_GeomSet2 Sint = M_GEOM_SET_EMPTY;
M_Real d12 = M_VecDistance2(C1.p, C2.p);
M_Real a, h, b;
M_Vector2 p;
M_Geom2 G1, G2;
if (Fabs(C1.p.x - C2.p.x) <= M_MACHEP &&
Fabs(C1.p.x - C2.p.x) <= M_MACHEP &&
Fabs(C1.r - C2.r) <= M_MACHEP) {
G1.type = M_CIRCLE;
G1.g.circle = C1;
M_GeomSetAdd2(&Sint, &G1);
return (Sint);
}
if (d12 > (C1.r + C2.r) ||
d12 < Fabs(C1.r - C2.r)) {
return (Sint);
}
a = (C1.r*C1.r - C2.r*C2.r + d12*d12) / (2.0*d12);
h = Sqrt(C1.r*C1.r - a*a);
p = M_VecLERP2(C1.p, C2.p, a/d12);
b = h/d12;
G1.type = M_POINT;
G1.g.point.x = p.x - b*(C2.p.y - C1.p.y);
G1.g.point.y = p.y + b*(C2.p.x - C1.p.x);
G2.type = M_POINT;
G2.g.point.x = p.x + b*(C2.p.y - C1.p.y);
G2.g.point.y = p.y - b*(C2.p.x - C1.p.x);
M_GeomSetAdd2(&Sint, &G1);
if (M_VecDistance2(G1.g.point, G2.g.point) > M_MACHEP) {
M_GeomSetAdd2(&Sint, &G2);
}
return (Sint);
}
FLOAT pow(FLOAT x, FLOAT y) {
/* we only compute x^0.333 */
FLOAT t2, dt, t = int2F(2);
do {
t2 = F_mul_F(t, t);
dt = (F_div_F(x, t2) - t) / 3;
t += dt;
} while(Fabs(dt) > f2F(1e-4));
return t;
}
FLOAT F_div_F(FLOAT aa, FLOAT bb) {
uint32_t a, b, sgn, res;
FLOAT ret;
int i;
a = Fabs(aa);
b = Fabs(bb);
sgn = (a != aa) ^ (b != bb);
nemu_assert(b != 0);
res = a / b;
a = a % b;
for(i=0; i<16; i++) {
res <<= 1;
a <<= 1;
if(a >= b) {
res |= 1;
a -= b;
}
}
res &= 0x7FFFFFFF;
ret = res;
return sgn ? -ret : ret;
}
void Tartet::Allocator(void)
{
if ((dx.data[0] != onex_) || (dx.data[1] != onex_))
Errmsg("Fatal", "Tartet", " tartet does not support noncubic lattice");
//
// Determine list of occupied sites.
int curSize = nx * ny;
ixyz.Dimension(curSize, 3);
nat0 = 0;
for(int i=minJx; i<=maxJx; ++i)
{
real x = i + xoff;
// YMAX=largest value of Y which can occur for this X value
// YMIN=smallest value of Y which can occur for this X value
// ZMAX0=largest value of Z which can occur for this X value
real ymax = (real)( shpar[0] * Sqrt(3./16.) - x / Sqrt(twox_));
real ymin = (real)(-shpar[0] * Sqrt(3./64.) + x / Sqrt(8.));
real zmax0 = (real)(3. * shpar[0] / 8. - x * Sqrt(3./8.));
for(int j=minJy; j<=maxJy; ++j)
{
real y = j + yoff;
if ((y >= ymin) && (y <= ymax))
{
real fy = (y - ymin) / (ymax - ymin);
real zmax = (onex_ - fy) * zmax0; // ! ZMAX=largest value of Z which can occur for this (X,Y)
for(int k=minJz; k<=maxJz; ++k)
{
real z = k + zoff;
if (Fabs(z) <= zmax) // ! Site is occupied:
{
if (nat0 >= curSize)
{
ixyz.Extend(nz);
curSize = ixyz.GetSize(0);
}
ixyz.Fill3(nat0, i, j, k);
int index = GetLinearAddress(nat0);
Composer(index, 0);
iocc[index] = true;
++nat0;
}
}
}
}
}
ixyz.Close(nat0);
}
/* multiply the accumulators
* by the inverse diagonal
* hessian (hess)
*/
void
Hess_acc(weight *nptr)
{
int i, iend;
weight *w;
if (nptr==NULL) {
w=weightbase;
iend=weightnombre;
} else {
w=nptr;
iend=1;
}
for ( i=0; i<iend; i++, w++ ) {
w->Wacc=Fdiv( w->Wacc, Fadd( mu, Fabs(w->Whess)));
}
}
/*
* Compute the shortest line segment connecting two lines in R^3.
* Adapted from Paul Bourke's example code:
* http://paulbourke.net/geometry/lineline3d
*/
int
M_LineLineShortest3(M_Line3 L1, M_Line3 L2, M_Line3 *Ls)
{
M_Vector3 p1 = M_LineInitPt3(L1);
M_Vector3 p2 = M_LineTermPt3(L1);
M_Vector3 p3 = M_LineInitPt3(L2);
M_Vector3 p4 = M_LineTermPt3(L2);
M_Vector3 p13, p43, p21;
M_Real d1343, d4321, d1321, d4343, d2121;
M_Real numer, denom;
M_Real muA, muB;
p13 = M_VecSub3(p1, p3);
p43 = M_VecSub3(p4, p3);
if (Fabs(p43.x) < M_MACHEP &&
Fabs(p43.y) < M_MACHEP &&
Fabs(p43.z) < M_MACHEP)
return (0);
p21 = M_VecSub3(p2, p1);
if (Fabs(p21.x) < M_MACHEP &&
Fabs(p21.y) < M_MACHEP &&
Fabs(p21.z) < M_MACHEP)
return (0);
d1343 = M_VecDot3p(&p13, &p43);
d4321 = M_VecDot3p(&p43, &p21);
d4343 = M_VecDot3p(&p43, &p43);
d2121 = M_VecDot3p(&p21, &p21);
denom = d2121*d4343 - d4321*d4321;
if (Fabs(denom) < M_MACHEP) {
return (0);
}
numer = d1343*d4321 - d1321*d4343;
muA = numer/denom;
muB = (d1343 + d4321*muA) / d4343;
if (Ls != NULL) {
*Ls = M_LineFromPts3(
M_VecAdd3(p1, M_VecScale3(p21,muA)),
M_VecAdd3(p3, M_VecScale3(p43,muB)));
}
return (1);
}
int main() {
// nemu_assert(int2F(1) + F_mul_int(F_mul_F(f2F(-0.8),f2F(-0.8)),25) > int2F(16.9));
// nemu_assert(int2F(1) + F_mul_int(F_mul_F(f2F(-0.8),f2F(-0.8)),25) < int2F(17));
// nemu_assert();
// nemu_assert(F_div_F(int2F(1) , int2F(1) + F_mul_int(F_mul_F(f2F(-0.8),f2F(-0.8)),25)) > f2F(0));
// nemu_assert(F_div_F(int2F(1) , f2F(17)) > f2F(0.05));
/* nemu_assert(F_div_int(int2F(2),10) <= f2F(0.2));
nemu_assert(F_div_int(int2F(2),10) > f2F(0.1999));
nemu_assert(F_mul_F(f2F(1.0),int2F(25)) == f2F(25));
nemu_assert(f(int2F(1)) < f2F(0.039));
nemu_assert(F_mul_int(f2F(0.2),9 ) > f2F(1.7));*/
// nemu_assert(F_mul_F(f2F(-0.8),f2F(-0.8)) == f2F(0.64));
// nemu_assert(F_div_F(0x10000,0x10ffcd) == int2F(0));
// nemu_assert(f(-0.8) > f2F(0.05));
// nemu_assert(F_mul_F(f2F(0.8),f2F(0.8)) < f2F(0.64));
// nemu_assert(f(f2F(0.8)) > f2F(0.05));
// nemu_assert(f(f2F(0.8)) < f2F(0.06));
// nemu_assert(F_mul_F(f2F(-0.2),f2F(-0.2)) == f2F(0.04));
// nemu_assert(f(f2F(-0.2)) == int2F(0));
// nemu_assert(f2F(-1.0) + F_mul_int(f2F(0.2),9) < f2F(0.9));
// nemu_assert(f(f2F(-1.0) + F_mul_int(f2F(0.2), 9)) > f2F(0.03));
// nemu_assert(f2F(0.2) == 0x3333);
// nemu_assert(F_mul_F(f2F(2.756),f2F(0.2)) > f2F(0.55));
// nemu_assert(F_mul_F(f2F(2.756),f2F(0.2)) < f2F(0.5515));
// nemu_assert( < f2F(0.05));
// nemu_assert(f(1.8)<f2F(0.040));
FLOAT a = computeT(10, f2F(-1.0), f2F(1.0), f);
// nemu_assert(a > int2F(1));
FLOAT ans = f2F(0.551222);
nemu_assert(Fabs(a - ans) < f2F(1e-4));
// nemu_assert(a < f2F(0.019));
// FLOAT ans = f2F(2.756);
//nemu_assert(a <= ans);
// nemu_assert(a > f2F(0.1));
// nemu_assert(Fabs(a - ans) < f2F(1e-4));
HIT_GOOD_TRAP;
return 0;
}
void Getmueller(DDscatParameters *param, int ibeta, int iphi, int itheta, PeriodicBoundaryFlag jpbc, real *orderm, real *ordern, real ak1,
Vect3<real> *aks_tf, Vect3<real> *ensc_lf, Vect3<real> *ensc_tf, real pyddx, real pzddx, real *sm, real *smori, real *s1111, real *s2121,
Complex *cx1121, FourArray *cxfData, SumPackage &sumPackage, Vect3<real> *em1_lf, Vect3<real> *em2_lf, Vect3<real> *em1_tf, Vect3<real> *em2_tf)
{
/* **
Given:
IBETA =index specifying rotation of target around axis a1
IORTH =number of incident polarization states being calculated for
IPHI =index specifying phi value for orientation of target axis a1
ITHETA=index specifying theta value for orientation of target axis a1
JPBC = 0 if PBC not used
1 if PBC used in y direction only
2 if PBC used in z direction only
3 if PBC used in both y and z directions
MXBETA=dimensioning information
MXSCA =dimensioning information
MXPHI =dimensioning information
MXTHET=dimensioning information
NSCAT =number of scattering directions for evaluation of Mueller matrix
CMDTRQ='DOTORQ' or 'NOTORQ' depending on whether or not torque calculation is to be carried out
ENSC_LF(1-3,1-NSCAT)=normalized scattering direction vector in Lab Frame for scattering directions 1-NSCAT
ENSCR(1-3,1-NSCAT)=normalized scattering direction vector in Target Frame for scattering directions 1-NSCA
EM1_LF(1-3,1-NSCAT)=unit scattering polarization vector 1 in Lab Frame
EM2_LF(1-3,1-NSCAT)=unit scattering polarization vector 2 in Lab Frame
EM1_TF(1-3,1-NSCAT)=unit scattering polarization vector 1 in Target Frame
EM2_TF(1-3,1-NSCAT)=unit scattering polarization vector 2 in Target Frame
PHIN(1-NSCAT)=values of PHI for scattering directions 1-NSCAT in Lab Frame [only used if JPBC=0: scattering by finite target]
AKS_TF(1-3,1-NSCAT)=(kx,ky,kz)*d for NSCAT scattering directions in Target Frame
ORDERM(1-NSCAT)=scattering order M for case of 1-d or 2-d target
ORDERN(1-NSCAT)=scattering order N for case of 2-d target
S1111(1-NSCAT)=weighted sum of |f_11|^2 over previous orientations
S2121(1-NSCAT)=weighted sum of |f_21|^2 over previous orientations
CXE01_LF(1-3) =incident pol state 1 in Lab Frame
CXE02_LF(1-3) = 2
CXF11(1-NSCAT)=f_11 for current orientation
CXF12(1-NSCAT)=f_12 for current orientation
CXF21(1-NSCAT)=f_21 for current orientation
CXF22(1-NSCAT)=f_22 for current orientation
QABS(2) =Q_abs for two incident polarizations
QABSUM(2) =weighted sum of Qabs over previous orientations
QBKSCA(2) =Q_bk for two incident polarizations
QBKSUM(2) =weighted sum of Q_bk over previous orientations
QEXSUM(2) =weighted sum of Q_ext over previous orientations
QEXT(2) =Q_ext for two incident polarizations
QPHA(2) =Q_pha for two incident polarizations
QPHSUM(2) =weighted sum of Q_ph over previous orientations
QSCAG(3,2) =Q_pr vector for two incident polarizations
QSCAT(2) =Q_sca for two incident polarizations
QSCGSUM(3,2) =weighted sum of Q_pr vector over prev. orients.
QSCSUM(2) =weighted sum of Q_sca over previous orientations
QTRQAB(3,2) =absorptive part of Q_gamma for two inc. pols.
QTRQABSUM(3,2)=weighted sum of Q_gamma over previous orientations
QTRQSC(3,2) =scattering part of Q_gamma for two inc. pols.
QTRQSCSUM(3,2)=weighted sum of Q_gamma over previous orientations
WGTA(ITHETA,IPHI)=weight factor for theta,phi orientation
WGTB(IBETA) =weight factor for beta orientation
SMORI(1-NSCAT,4,4)=weighted sum of Mueller matrix over previous orientations
If IORTH=1, returns:
S1111(1-NSCAT)=updated weighted sum of |f_11|^2 over NSCAT scattering directions
S2121(1-NSCAT)=updated weighted sum of |f_21|^2
CX1121(1-NSCAT)=updated weighted summ of f_11*conjg(f_21)
If IORTH=2, returns:
SM(1-NSCAT,4,4)=Mueller matrix for current orientation, and NSCAT scattering directions
CXS1(1-NSCAT)=amplitude scattering matrix element S_1 for current orientation
CXS2(1-NSCAT)=amplitude scattering matrix element S_2 for current orientation
CXS3(1-NSCAT)=amplitude scattering matrix element S_3 for current orientation
CXS4(1-NSCAT)=amplitude scattering matrix element S_4 for current orientations
QABSUM(2) =updated weighted sum of Q_abs
QBKSUM(2) =updated weighted sum of Q_bk
QEXSUM(2) =updated weighted sum of Q_ext
QSCGSUM(3,2) =updated weighted sum of Q_pr vector
QSCSUM(2) =updated weighted sum of Q_sca
QTRQABSUM(3,2)=updated weighted sum of absorptive contribution to Q_gamma
QTRQSCSUM(3,2)=updated weighted sum of scattering contribution to Q_gamma
SMORI(1-NSCAT,4,4)=updated weighted sum of Mueller matrix elements
History:
Fortran versions history removed.
Copyright (C) 1996,1997,1998,2000,2003,2006,2007,2008,2011,2012
B.T. Draine and P.J. Flatau
This code is covered by the GNU General Public License.
** */
/* **
!********* Sum scattering properties over orientations ****************
Correspondence to notation of Bohren & Huffman (1983):
Our f_jk correspond to notation of Draine (1988).
For the special case where incident polarization modes 1 and 2 are
arallel and perpendicular to the scattering plane, we have the
relationship between the f_jk and the elements S_j of the amplitude
scattering matrix as defined by Bohren & Huffman (1983) is as follows:
S_1 = -i*f_22
S_2 = -i*f_11
S_3 = i*f_12
S_4 = i*f_21
In this case (incident pol. states parallel,perpendicular to the
scattering plane), the elements of the 4x4 Mueller matrix (see Bohren
Huffman 1983) are related to our f_jk as follows:
S_11 = [|f_11|^2 + |f_22|^2 + |f_12|^2 + |f_21|^2]/2
S_12 = [|f_11|^2 + |f_21|^2 - |f_22|^2 - |f_12|^2]/2
S_13 = -Re[f_11*conjg(f_12) + f_22*conjg(f_21)]
S_14 = Im[f_22*conjg(f_21) - f_11*conjg(f_12)]
S_21 = [|f_11|^2 + |f_12|^2 - |f_22|^2 - |f_21|^2]/2
S_22 = [|f_11|^2 + |f_22|^2 - |f_21|^2 - |f_12|^2]/2
S_23 = Re[f_22*conjg(f_21) - f_11*conjg(f_12)]
S_24 = -Im[f_11*conjg(f_12) + f_22*conjg(f_21)]
S_31 = -Re[f_11*conjg(f_21) + f_22*conjg(f_22)]
S_32 = Re[f_22*conjg(f_12) - f_22*conjg(f_21)]
S_33 = Re[f_22*conjg(f_11) + f_12*conjg(f_21)]
S_34 = Im[f_11*conjg(f_22) + f_21*conjg(f_22)]
S_41 = -Im[f_21*conjg(f_11) + f_22*conjg(f_12)]
S_42 = Im[f_22*conjg(f_12) - f_22*conjg(f_11)]
S_43 = Im[f_22*conjg(f_11) - f_12*conjg(f_21)]
S_44 = Re[f_22*conjg(f_11) - f_12*conjg(f_21)]
Notation internal to this program:
ij.ne.kl: CX_ijkl = \f_ij* \times f_kl (summed over orientations)
S_ijij = \f_ij* \times f_ij (summed over orientations)
4 pure real elements: S1111,S1212,S2121,S2222
8 Complex elements: CX1112,CX1121,CX1122,CX1221,CX1222,CX2122
4 redundant elts.: CX1211 = Conjg(CX1112)
CX2111 = Conjg(CX1121)
CX2112 = Conjg(CX1221)
CX2211 = Conjg(CX1122)
In this routine we will carry out fully general calculation of the
4x4 Mueller matrix elements, valid for arbitrary incident polarization
states j=1,2 for which we have the scattering matrix f_ij
First, we compute the amplitude scattering matrix elements S_1,S_2,
S_3,S_4 as defined by Bohren & Huffman:
** */
// Notation:
// khat_0 = unit vector along incident direction
// ehat_01 = CXE01 = complex incident polarization vector 1
// ehat_02 = CXE02 = complex incident polarization vector 2
// ehat_0perp = incident pol vector perp to scattering plane scattered pol vector perp to scattering plane em2
// ehat_0para = incident pol vector para to scattering plane
// = khat_0 cross ehat_0perp [conventional def.]
// = xhat_LF cross ehat_0perp
// = xhat_LF cross [yhat_LF (yhat_LF dot ehat_0perp) + zhat_LF (zhat_LF dot ehat_0perp)]
// = - yhat_LF (ehat_0perp dot zhat_LF) + zhat_LF (ehat_0perp dot yhat_LF) - yhat_LF em2_LF(3) + zhat_LF em2_LF(2)
// em2 = scattered pol vector perp to scattering plane
// cxa = conjg(ehat_01) dot yhat_LF
// cxb = conjg(ehat_01) dot zhat_LF
// cxc = conjg(ehat_02) dot yhat_LF
// cxd = conjg(ehat_02) dot zhat_LF
//
// WG=weighting factor for this orientation
const Complex conei((real)0., (real)1.);
const real half_ = (real)0.5;
Complex cxa, cxb, cxc, cxd, cxfac0, cxfac1, cxfac2;
real fac;
int nd, k;
real wg = param->GetOriData()->GetWgtaValue(itheta, iphi) * param->GetOriData()->GetWgtb(ibeta);
if (param->Iorth() == 2)
{
FourArray *myData = new FourArray;
myData->Alloc(param->Nscat());
Complex *myS1 = myData->Cxs1();
Complex *myS2 = myData->Cxs2();
Complex *myS3 = myData->Cxs3();
Complex *myS4 = myData->Cxs4();
//
// Compute Complex scattering amplitudes S_1,S_2,S_3,S_4 for this particular target orientation and NSCAT scattering directions:
switch(jpbc) // ChB: TODO the code snippets are identical for every jpbc
{
case PeriodicNo:
cxa = param->Cxe01_lf().data[1].conjg();
cxb = param->Cxe01_lf().data[2].conjg();
cxc = param->Cxe02_lf().data[1].conjg();
cxd = param->Cxe02_lf().data[2].conjg();
for(nd=0; nd<param->Nscat(); ++nd)
{
real cosphi = em2_lf[nd].data[2];
real sinphi = -em2_lf[nd].data[1];
Complex cxaa = cxa * cosphi + cxb * sinphi;
Complex cxbb = cxb * cosphi - cxa * sinphi;
Complex cxcc = cxc * cosphi + cxd * sinphi;
Complex cxdd = cxd * cosphi - cxc * sinphi;
myS1[nd] = -conei * (cxfData->Cx21(nd) * cxbb + cxfData->Cx22(nd) * cxdd);
myS2[nd] = -conei * (cxfData->Cx11(nd) * cxaa + cxfData->Cx12(nd) * cxcc);
myS3[nd] = conei * (cxfData->Cx11(nd) * cxbb + cxfData->Cx12(nd) * cxdd);
myS4[nd] = conei * (cxfData->Cx21(nd) * cxaa + cxfData->Cx22(nd) * cxcc);
}
break;
case PeriodicY:
case PeriodicZ:
// JPBC = 1 or 2:
// ENSC(1-3,ND) = components of scattering unit vector in Lab Frame
// CXA,CXB = y,z components of incident polarization state 1 in Lab Frame
// CXC,CXD = y,z 2 in Lab Frame
cxa = param->Cxe01_lf().data[1].conjg();
cxb = param->Cxe01_lf().data[2].conjg();
cxc = param->Cxe02_lf().data[1].conjg();
cxd = param->Cxe02_lf().data[2].conjg();
for(nd=0; nd<param->Nscat(); ++nd)
{
real cosphi = em2_lf[nd].data[2];
real sinphi = -em2_lf[nd].data[1];
Complex cxaa = cxa * cosphi + cxb * sinphi;
Complex cxbb = cxb * cosphi - cxa * sinphi;
Complex cxcc = cxc * cosphi + cxd * sinphi;
Complex cxdd = cxd * cosphi - cxc * sinphi;
myS1[nd] = -conei * (cxfData->Cx21(nd) * cxbb + cxfData->Cx22(nd) * cxdd);
myS2[nd] = -conei * (cxfData->Cx11(nd) * cxaa + cxfData->Cx12(nd) * cxcc);
myS3[nd] = conei * (cxfData->Cx11(nd) * cxbb + cxfData->Cx12(nd) * cxdd);
myS4[nd] = conei * (cxfData->Cx21(nd) * cxaa + cxfData->Cx22(nd) * cxcc);
}
break;
case PeriodicBoth:
//fprintf(stderr, "Getmueller %p\n", cxfData);
//
// JPBC = 3
// Target is periodic in y and z directions in Lab Frame Allowed scattering directions are identified by scattering order (M,N).
// For each allowed (M,N) there is one forward (transmitted) direction and one backward (reflected) direction.
// Following convention set in subroutine PBCSCAVEC for JPBC=3, it is assumed that NSCAT is even, with the first NSCAT/2 directions
// corresponding to transmission, and the remaining NSCAT/2 directions corresponding to reflection
//
// ENSC(1-3,ND) = components of scattering unit vector in Lab Frame
// EM1(1-3,ND) = components of scattering polarization 1 in Lab Frame
// EM2(1-3,ND) = components of scattering polarization 2 in Lab Frame
cxa = param->Cxe01_lf().data[1].conjg();
cxb = param->Cxe01_lf().data[2].conjg();
cxc = param->Cxe02_lf().data[1].conjg();
cxd = param->Cxe02_lf().data[2].conjg();
for(nd=0; nd<param->Nscat(); ++nd)
{
real cosphi = em2_lf[nd].data[2];
real sinphi = -em2_lf[nd].data[1];
Complex cxaa = cxa * cosphi + cxb * sinphi;
Complex cxbb = cxb * cosphi - cxa * sinphi;
Complex cxcc = cxc * cosphi + cxd * sinphi;
Complex cxdd = cxd * cosphi - cxc * sinphi;
myS1[nd] = -conei * (cxfData->Cx21(nd) * cxbb + cxfData->Cx22(nd) * cxdd);
myS2[nd] = -conei * (cxfData->Cx11(nd) * cxaa + cxfData->Cx12(nd) * cxcc);
myS3[nd] = conei * (cxfData->Cx11(nd) * cxbb + cxfData->Cx12(nd) * cxdd);
myS4[nd] = conei * (cxfData->Cx21(nd) * cxaa + cxfData->Cx22(nd) * cxcc);
//fprintf(stderr, "%lf %lf %lf %lf %lf %lf %lf %lf\n", cxfData->Cx11(nd).re, cxfData->Cx11(nd).im, cxfData->Cx12(nd).re, cxfData->Cx12(nd).im,
// cxfData->Cx21(nd).re, cxfData->Cx21(nd).im, cxfData->Cx22(nd).re, cxfData->Cx22(nd).im);
//fprintf(stderr, "%lf %lf %lf %lf %lf %lf %lf %lf\n", myS1[nd].re, myS1[nd].im, myS2[nd].re, myS2[nd].im, myS3[nd].re, myS3[nd].im, myS4[nd].re, myS4[nd].im);
}
break;
default:
break;
}
//
// if JPBC=1,2,3 (target periodic in y, z, or y and z directions):
// Check for special case: JPBC=3 (2-d target) and forward scattering wit
// ORDERM=ORDERN=0. For this case we need to add incident wave to
// S_1 and S_2.
// Note that S_1 and S_2 are defined such that for M=N=0 we change
// iS_1 -> iS_1 +1 and iS_2 -> iS_2 +1
// S_1 -> S_1 -i S_2 -> S_2 -i
// note that CXS1 and CXS2 have yet to be multiplied by factor
// 2*pi/(ak1*aksr(1,nd)*pyddx*pzddx)
if (jpbc == PeriodicBoth)
{
for(nd=0; nd<param->Nscat() / 2; ++nd)
{
if ((nint_(orderm[nd]) == 0) && (nint_(ordern[nd]) == 0))
{
// Zero-degree forward-scattering need to add incident wave to radiated wave
// 2012.04.26 (BTD) change
// ! CXFAC0=AKS_TF(1,ND)*AK1*PYDDX*PZDDX/(2._WP*PI)
cxfac0 = Complex(Fabs(aks_tf[nd].data[0]) * ak1 * pyddx * pzddx / TwoPi, (real)0.);
// ! 2012.04.25 (BTD): for reasons not understood, incident wave added for S2 has different sign from S1
// ! ... does this indicate a sign mistake in calculation of either S1 or S2?
// ! ... is it possible that there is an error in eq. 68 of Draine & Flatau 2008?. Something like this could
// arise from a 180deg shift in directions between
// ! the incident and scattered "parallel" basis vectors, or between the incident and scattered "perpendicular" basis vectors (should check for this in output files!)
// ! This appears to indicate that for JPBC=3 the
// ! sign of either CXS1 or CXS2 is incorrect
// ! The Mueller matrix elements S11, S22, S12, and S21 appear
// ! to be correct, but they would not be affected by
// ! a sign error in either CXS1 or CXS2
// ! Elements S13,S14,S23,S24,S31,S32,S33,S34,S41,S42,S43,S44
// ! would be sensitive to a sign error, and it would be
// ! a good idea to find a way to test them.
myS1[nd] -= cxfac0;
myS2[nd] += cxfac0;
//fprintf(stderr, "%d %lf %lf\n", nd, cxfac0.re, cxfac0.im);
}
}
}
//
// Calculation of scattering amplitudes CXS1, CXS2, CXS3, CXS4 is complete.
// Now compute Mueller matrix elements for this particular target orientation:
int index;
for(nd=0; nd<param->Nscat(); ++nd)
{
index = 16 * nd;
sm[index + 0] = half_ * (myS1[nd] * myS1[nd].conjg() + myS2[nd] * myS2[nd].conjg() + myS3[nd] * myS3[nd].conjg() + myS4[nd] * myS4[nd].conjg()).re;
sm[index + 1] = half_ * (myS2[nd] * myS2[nd].conjg() - myS1[nd] * myS1[nd].conjg() + myS4[nd] * myS4[nd].conjg() - myS3[nd] * myS3[nd].conjg()).re;
sm[index + 2] = (myS2[nd] * myS3[nd].conjg() + myS1[nd] * myS4[nd]).re;
sm[index + 3] = (myS2[nd] * myS3[nd].conjg() - myS1[nd] * myS4[nd]).im;
sm[index + 4] = half_ * (myS2[nd] * myS2[nd].conjg() - myS1[nd] * myS1[nd].conjg() - myS4[nd] * myS4[nd].conjg() + myS3[nd] * myS3[nd].conjg()).re;
sm[index + 5] = half_ * (myS2[nd] * myS2[nd].conjg() + myS1[nd] * myS1[nd].conjg() - myS4[nd] * myS4[nd].conjg() - myS3[nd] * myS3[nd].conjg()).re;
sm[index + 6] = (myS2[nd] * myS3[nd].conjg() - myS1[nd] * myS4[nd].conjg()).re;
sm[index + 7] = (myS2[nd] * myS3[nd].conjg() + myS1[nd] * myS4[nd].conjg()).im;
sm[index + 8] = (myS2[nd] * myS4[nd].conjg() + myS1[nd] * myS3[nd].conjg()).re;
sm[index + 9] = (myS2[nd] * myS4[nd].conjg() - myS1[nd] * myS3[nd].conjg()).re;
sm[index + 10] = (myS1[nd] * myS2[nd].conjg() + myS3[nd] * myS4[nd].conjg()).re;
sm[index + 11] = (myS2[nd] * myS1[nd].conjg() + myS4[nd] * myS3[nd].conjg()).im;
sm[index + 12] = (myS4[nd] * myS2[nd].conjg() + myS1[nd] * myS3[nd].conjg()).im;
sm[index + 13] = (myS4[nd] * myS2[nd].conjg() - myS1[nd] * myS3[nd].conjg()).im;
sm[index + 14] = (myS1[nd] * myS2[nd].conjg() - myS3[nd] * myS4[nd].conjg()).im;
sm[index + 15] = (myS1[nd] * myS2[nd].conjg() - myS3[nd] * myS4[nd].conjg()).re;
}
//
real fac0 = (real)0.;
if (jpbc != PeriodicNo)
{
switch(jpbc)
{
case PeriodicY: // Prepare to calculate S^{(1d)} for target with 1-d periodicity in y
fac0 = TwoPi / (ak1 * pyddx * pyddx);
break;
case PeriodicZ: // Prepare to calculate S^{(1d)} for target with 1-d periodicity in z
fac0 = TwoPi / (ak1 * pzddx * pzddx);
break;
case PeriodicBoth: // Prepare to calculate S^{(2d)} for target with periodicity in y and z
fac0 = TwoPi / (ak1 * pyddx * pzddx);
fac0 = fac0 * fac0;
break;
default:
break;
}
//
// k*sin(alpha_s)=component of k_s perpendicular to target
for(nd=0; nd<param->Nscat(); ++nd)
{
switch(jpbc)
{
case PeriodicY:
fac = fac0 / Sqrt(aks_tf[nd].data[0] * aks_tf[nd].data[0] + aks_tf[nd].data[2] * aks_tf[nd].data[2]);
break;
case PeriodicZ:
fac = fac0 / Sqrt(aks_tf[nd].data[0] * aks_tf[nd].data[0] + aks_tf[nd].data[1] * aks_tf[nd].data[1]);
break;
case PeriodicBoth:
fac = fac0 / (aks_tf[nd].data[0] * aks_tf[nd].data[0]);
break;
default:
Errmsg("Fatal", "Getmueller", " invalid jpbc");
break;
}
index = 16 * nd;
for(k=0; k<16; ++k)
{
sm[index + k] *= fac;
}
}
}
//
// Augment Mueller matrix for orientational average
index = 16 * param->Nscat();
for(nd=0; nd<index; ++nd)
{
smori[nd] += sm[nd] * wg;
}
delete myData;
}
else
{
//
// When IORTH=1, cannot compute full Mueller matrix. Compute three scattering properties:
for(nd=0; nd<param->Nscat(); ++nd)
{
s1111[nd] += (cxfData->Cx11(nd).conjg() * cxfData->Cx11(nd)).re * wg;
s2121[nd] += (cxfData->Cx21(nd).conjg() * cxfData->Cx21(nd)).re * wg;
cx1121[nd] += (cxfData->Cx11(nd).conjg() * cxfData->Cx21(nd)) * wg;
}
}
//
// Now augment sums of QABS,QBKSCA,QEXT,QSCA,G*QSCA over incident directions and polarizations.
for(int jo=0; jo<param->Iorth(); ++jo)
{
sumPackage.Qext().Qsum1()[jo] += sumPackage.Qext().Q()[jo] * wg;
sumPackage.Qabs().Qsum1()[jo] += sumPackage.Qabs().Q()[jo] * wg;
sumPackage.Qbksca().Qsum1()[jo] += sumPackage.Qbksca().Q()[jo] * wg;
sumPackage.Qpha().Qsum1()[jo] += sumPackage.Qpha().Q()[jo] * wg;
sumPackage.Qsca().Qsum1()[jo] += sumPackage.Qsca().Q()[jo] * wg;
sumPackage.Qscag2().Qsum1()[jo] += sumPackage.Qscag2().Q()[jo] * wg;
sumPackage.Qscag().Qsum1()[jo] += sumPackage.Qscag().Q()[jo] * wg;
if (DDscatParameters::GetInstance()->Cmdtrq() == TorqMethod_DOTORQ)
{
sumPackage.Qtrqab().Qsum1()[jo] += sumPackage.Qtrqab().Q()[jo] * wg;
sumPackage.Qtrqsc().Qsum1()[jo] += sumPackage.Qtrqsc().Q()[jo] * wg;
}
}
}
static inline Lisp_Object
_ase_metric_euclidean_1dim_fast(Lisp_Object a, Lisp_Object b)
{
return Fabs(ent_binop(ASE_BINARY_OP_DIFF, a, b));
}
virtual void solve(int steps = 100000, T tolerance = (T)1e-6){
cgfassert(this->mat->getWidth() == this->b->getSize());
cgfassert(this->mat->getWidth() == this->mat->getHeight());
cgfassert(this->mat->getWidth() == this->x->getSize());
this->iterations = 0;
for(int i=0;i<this->mat->getHeight();i++){
(*C)[i] = (T)1.0/Sqrt(Fabs((*this->mat)[i][i]));
}
/*r = Ax*/
spmv(*r, *(this->mat), *(this->x));
/*r = b - r*/;
Vector<T>::sub(*r, *(this->b), *r);
/*w = C * r*/
Vector<T>::mul(*w, *C, *r);
/*v = C * w*/
Vector<T>::mul(*v, *C, *w);
/*s1 = w * w*/
Vector<T>::mul(*scratch1, *w, *w);
T alpha;
/*alpha = sum(s1)*/
alpha = scratch1->sum();
int k=0;
while(k<steps){
/*s1 = v * v*/
this->iterations++;
Vector<T>::mul(*scratch1, *v, *v);
T residual;
residual = scratch1->sum();
if(Sqrt(Fabs(residual)) < (tolerance*bnorm /*+ tolerance*/)){
warning("CG::Success in %d iterations, %10.10e, %10.10e", k, residual, Sqrt(residual));
return;
}
/*u = A*v*/
spmv(*u, *(this->mat), *v);
/*s1 = v * u*/
Vector<T>::mul(*scratch1, *v, *u);
T divider;
/*divider = sum(s1)*/
divider = scratch1->sum();
T t = alpha/divider;
/*x = x + t*v*/
/*r = r - t*u*/
/*w = C * r*/
Vector<T>::mfadd(*(this->x), t, *v, *(this->x));
Vector<T>::mfadd(*r, -t, *u, *r);
Vector<T>::mul (*w, *C, *r);
/*s1 = w*w*/
Vector<T>::mul(*scratch1, *w, *w);
/*beta = sum(s1)*/
T beta = scratch1->sum();
#if 1
if(beta < (tolerance*bnorm + tolerance)){
T rl = r->length2();
if(Sqrt(rl)<(tolerance*bnorm + tolerance)){
warning("CG::Success in %d iterations, %10.10e, %10.10e", k, rl,
Sqrt(Fabs(rl)));
return;
}
}
#endif
T s = beta/alpha;
/*s1 = C * w*/
Vector<T>::mul(*scratch1, *C, *w);
/*v = s1 + s * v*/
Vector<T>::mfadd(*v, s, *v, *scratch1);
alpha = beta;
k++;
}
message("Unsuccesfull");
throw new SolutionNotFoundException(__LINE__, __FILE__,
"Number of iterations exceeded.");
}
State collideObjects(PhysicsObject* a, PhysicsObject* b){
State state;
state.deltaAngVel = 0;
state.deltaPosition = pointMake(0, 0);
state.deltaVel = pointMake(0, 0);
CollisionPair pairs[2*PHYSICS_OBJECT_MAXIMUM_POINTS];
if(!coarseCollision(a, b)){
return state;
}
//if a coarse collision happened, we will search for a refined one in each of A's edges.
//in this step we try to find every point where B has hitted A and A has hitted B
int Acurrent, Anext, Abefore, Bcurrent, Bnext, pairCount=0;
//first case---------------------------------------------------------------------------
/*
PA1 PA3
\ /
\ /
PB2---------PB1
\/
PA2
V=A --=B
*/
for (Acurrent=0; Acurrent<a->format.polygonInfo.count; Acurrent++) {
/*Given 3 sequenced vertex, we create two vectors, one from the
first to the second vertex and other from the second to the
third vertex.*/
Anext = Acurrent==a->format.polygonInfo.count-1?0:Acurrent+1;
Abefore = Acurrent==0?a->format.polygonInfo.count-1:Acurrent-1;
Point PA1 = worldPosition(a->format.polygonInfo.points[Abefore],*a);
Point PA2 = worldPosition(a->format.polygonInfo.points[Acurrent],*a);
Point PA3 = worldPosition(a->format.polygonInfo.points[Anext],*a);
Vector A1 = pointMake(PA2.x-PA1.x, PA2.y-PA1.y);
Vector A2 = pointMake(PA3.x-PA2.x, PA3.y-PA2.y);
pairs[pairCount].depth = 0;
pairs[pairCount].normal = pointMake(0, 0);
for (Bcurrent = 0; Bcurrent<b->format.polygonInfo.count; Bcurrent++) {
/*for each edge of B, we will find if any of the edges are hitted by both
edges defined before, if thats the case, then A hitted B on that point
If both edges of A hitted more than one edge of B, we will keep the highest
depth*/
Bnext = Bcurrent==b->format.polygonInfo.count-1?0:Bcurrent+1;
Point PB1 = worldPosition(b->format.polygonInfo.points[Bcurrent], *b);
Point PB2 = worldPosition(b->format.polygonInfo.points[Bnext], *b);
Vector B = pointMake(PB2.x-PB1.x, PB2.y-PB1.y);
Vector I1 = pointMake(PB1.x-PA1.x, PB1.y-PA1.y);
Vector I2 = pointMake(PB1.x-PA2.x, PB1.y-PA2.y);
float crossBI1 = B.x*I1.y-B.y*I1.x;
float crossBI2 = B.x*I2.y-B.y*I2.x;
float crossAI1 = A1.x*I1.y-A1.y*I1.x;
float crossAI2 = A2.x*I2.y-A2.y*I2.x;
float crossBA1 = B.x*A1.y-B.y*A1.x;
float crossBA2 = B.x*A2.y-B.y*A2.x;
crossBA1=crossBA1==0?0.0001:crossBA1;
crossBA2=crossBA2==0?0.0001:crossBA2;
float t1 = crossAI1/crossBA1;
float w1 = crossBI1/crossBA1;
float t2 = crossAI2/crossBA2;
float w2 = crossBI2/crossBA2;
if(t1>0 && t1<1 && w1>0 && w1<1 && t2>0 && t2<1 && w2>0 && w2<1){
//we do have a collision
Vector normal = rotateVector(B, -PI*0.5); //rotate the edge by -90 degrees, so that it points outside
normalizePoint(&normal);
Point collisionPoint=pointMake(((PA1.x+w1*A1.x)+(PA2.x+w2*A2.x))*0.5, ((PA1.y+w1*A1.y)+(PA2.y+w2*A2.y))*0.5);
float depth = (PA2.x-collisionPoint.x)*normal.x+(PA2.y-collisionPoint.y)*normal.y;
depth = -depth;
if(Fabs(depth)>Fabs(pairs[pairCount].depth)){
pairs[pairCount].depth = depth;
pairs[pairCount].normal = normal;
pairs[pairCount].location=collisionPoint;
}
}
}
if(Fabs(pairs[pairCount].depth)>0){
pairCount++;
}
}
//second case---------------------------------------------------------------------------
/*
PA1 PA3
\ /
\ /
PB2---------PB1
\/
PA2
V=B --=A
*/
//Although we did not change the names, A variables now refers to B while B variables refer to A
for (Acurrent=0; Acurrent<b->format.polygonInfo.count; Acurrent++) {
/*Given 3 sequenced vertex, we create two vectors, one from the
first to the second vertex and other from the second to the
third vertex.*/
Anext = Acurrent==b->format.polygonInfo.count-1?0:Acurrent+1;
Abefore = Acurrent==0?b->format.polygonInfo.count-1:Acurrent-1;
Point PA1 = worldPosition(b->format.polygonInfo.points[Abefore],*b);
Point PA2 = worldPosition(b->format.polygonInfo.points[Acurrent],*b);
Point PA3 = worldPosition(b->format.polygonInfo.points[Anext],*b);
Vector A1 = pointMake(PA2.x-PA1.x, PA2.y-PA1.y);
Vector A2 = pointMake(PA3.x-PA2.x, PA3.y-PA2.y);
pairs[pairCount].depth = 0;
pairs[pairCount].normal = pointMake(0, 0);
for (Bcurrent = 0; Bcurrent<a->format.polygonInfo.count; Bcurrent++) {
/*for each edge of A, we will find if any of the edges are hitted by both
edges defined before, if thats the case, then A hitted B on that point
If both edges of B hitted more than one edge of A, we will keep the highest
depth*/
Bnext = Bcurrent==a->format.polygonInfo.count-1?0:Bcurrent+1;
Point PB1 = worldPosition(a->format.polygonInfo.points[Bcurrent], *a);
Point PB2 = worldPosition(a->format.polygonInfo.points[Bnext], *a);
Vector B = pointMake(PB2.x-PB1.x, PB2.y-PB1.y);
Vector I1 = pointMake(PB1.x-PA1.x, PB1.y-PA1.y);
Vector I2 = pointMake(PB1.x-PA2.x, PB1.y-PA2.y);
float crossBI1 = B.x*I1.y-B.y*I1.x;
float crossBI2 = B.x*I2.y-B.y*I2.x;
float crossAI1 = A1.x*I1.y-A1.y*I1.x;
float crossAI2 = A2.x*I2.y-A2.y*I2.x;
float crossBA1 = B.x*A1.y-B.y*A1.x;
float crossBA2 = B.x*A2.y-B.y*A2.x;
crossBA1=crossBA1==0?0.0001:crossBA1;
crossBA2=crossBA2==0?0.0001:crossBA2;
float t1 = crossAI1/crossBA1;
float w1 = crossBI1/crossBA1;
float t2 = crossAI2/crossBA2;
float w2 = crossBI2/crossBA2;
if(t1>0 && t1<1 && w1>0 && w1<1 && t2>0 && t2<1 && w2>0 && w2<1){
//we do have a collision
Vector normal = rotateVector(B, -PI*0.5); //rotate the edge by -90 degrees, so that it points outside
normalizePoint(&normal);
Point collisionPoint=pointMake(((PA1.x+w1*A1.x)+(PA2.x+w2*A2.x))*0.5, ((PA1.y+w1*A1.y)+(PA2.y+w2*A2.y))*0.5);
float depth = (PA2.x-collisionPoint.x)*normal.x+(PA2.y-collisionPoint.y)*normal.y;
if(Fabs(depth)>Fabs(pairs[pairCount].depth)){
pairs[pairCount].depth = depth;
pairs[pairCount].normal = normal;
pairs[pairCount].location=collisionPoint;
}
}
}
if(Fabs(pairs[pairCount].depth)>0){
pairCount++;
}
}
//third case---------------------------------------------------------------------------
/*
PA1 PA3
\ PB2 /
\ /\ /
X X
/ \/ \
/ PA2 \
PB3 PB1
V=A /\=B
*/
int Bbefore;
for (Acurrent=0; Acurrent<a->format.polygonInfo.count; Acurrent++) {
/*Given 3 sequenced vertex, we create two vectors, one from the
first to the second vertex and other from the second to the
third vertex.*/
Anext = Acurrent==a->format.polygonInfo.count-1?0:Acurrent+1;
Abefore = Acurrent==0?a->format.polygonInfo.count-1:Acurrent-1;
Point PA1 = worldPosition(a->format.polygonInfo.points[Abefore],*a);
Point PA2 = worldPosition(a->format.polygonInfo.points[Acurrent],*a);
Point PA3 = worldPosition(a->format.polygonInfo.points[Anext],*a);
Vector A12 = pointMake(PA2.x-PA1.x, PA2.y-PA1.y);
Vector A23 = pointMake(PA3.x-PA2.x, PA3.y-PA2.y);
pairs[pairCount].depth = 0;
pairs[pairCount].normal = pointMake(0, 0);
for (Bcurrent = 0; Bcurrent<b->format.polygonInfo.count; Bcurrent++) {
/*for each pair of edge of B, we will find if any of the edges are hitted as shown above*/
Bnext = Bcurrent==b->format.polygonInfo.count-1?0:Bcurrent+1;
Bbefore = Bcurrent==0?b->format.polygonInfo.count-1:Bcurrent-1;
Point PB1 = worldPosition(b->format.polygonInfo.points[Bbefore], *b);
Point PB2 = worldPosition(b->format.polygonInfo.points[Bcurrent], *b);
Point PB3 = worldPosition(b->format.polygonInfo.points[Bnext], *b);
Vector B12 = pointMake(PB2.x-PB1.x, PB2.y-PB1.y);
Vector B23 = pointMake(PB3.x-PB2.x, PB3.y-PB2.y);
Vector IA12B23 = pointMake(PB2.x-PA1.x, PB2.y-PA1.y);
Vector IA23B12 = pointMake(PB1.x-PA2.x, PB1.y-PA2.y);
float crossB12I = B12.x*IA23B12.y-B12.y*IA23B12.x;
float crossB23I = B23.x*IA12B23.y-B23.y*IA12B23.x;
float crossA12I = A12.x*IA12B23.y-A12.y*IA12B23.x;
float crossA23I = A23.x*IA23B12.y-A23.y*IA23B12.x;
float crossB12A23 = B12.x*A23.y-B12.y*A23.x;
float crossB23A12 = B23.x*A12.y-B23.y*A12.x;
crossB23A12=crossB23A12==0?0.0001:crossB23A12;
crossB12A23=crossB12A23==0?0.0001:crossB12A23;
float t1 = crossA12I/crossB23A12;
float w1 = crossB23I/crossB23A12;
float t2 = crossA23I/crossB12A23;
float w2 = crossB12I/crossB12A23;
if(t1>0 && t1<1 && w1>0 && w1<1 && t2>0 && t2<1 && w2>0 && w2<1){
//we do have a collision
Point collision1 = pointMake((PA1.x+w1*A12.x), (PA1.y+w1*A12.y));
Point collision2 = pointMake((PA2.x+w2*A23.x), (PA2.y+w2*A23.y));
Vector c12 = pointMake(collision2.x-collision1.x, collision2.y-collision1.y);
Vector normal = rotateVector(c12, -PI*0.5); //rotate the edge by -90 degrees, so that it points outside
normalizePoint(&normal);
Point collisionPoint=pointMake((collision1.x+collision2.x)*0.5, (collision1.y+collision2.y)*0.5);
float depth = (PA2.x-PB2.x)*normal.x+(PA2.y-PB2.y)*normal.y;
depth = -depth;
pairs[pairCount].depth = depth;
pairs[pairCount].normal = normal;
pairs[pairCount].location=collisionPoint;
pairCount++;
}
}
}
int currentCollision;
//solves the physics part ot the collision for each collision that has happened
for (currentCollision =0; currentCollision<pairCount; currentCollision++) {
State st = physicsResolution(a,b,pairs[currentCollision]);
state.deltaAngVel += st.deltaAngVel;
state.deltaPosition=pointMake(st.deltaPosition.x+state.deltaPosition.x, st.deltaPosition.y+state.deltaPosition.y);
state.deltaVel=pointMake(st.deltaVel.x+state.deltaVel.x, st.deltaVel.y+state.deltaVel.y);
}
state.deltaAngVel += a->angularVelocity;
state.deltaVel = pointMake(a->linearVelocity.x+state.deltaVel.x, a->linearVelocity.y+state.deltaVel.y);
state.deltaPosition = pointMake(a->position.x+state.deltaPosition.x, a->position.y+state.deltaPosition.y);
return state;
}
) * 0.5f ; v3Extent . z = Fabs ( llllll\ ll\ l\ . z - lO\ OOOOOOO\ O\ . z ) * 0.5f ; } AABB AABB :: operator\
// determine centroid XCM,YCM,ZCM
// If A/d_x is near an odd int, XCM=0, even, 0.5
// If B/d_y is near an odd int, YCM=0, even, 0.5
// If B/d_z is near an odd int, ZCM=0, even, 0.5
void Tarcyl::PreSizer(int jx, int jy, int jz)
{
xcm = (jx%2) ? zero_ : half_;
ycm = (jy%2) ? zero_ : half_;
zcm = (jz%2) ? zero_ : half_;
real r2 = (real)0.25 * shpar[1] * shpar[1];
minJx = jx + 1;
maxJx = -jx;
minJy = jy + 1;
maxJy = -jy;
minJz = jz + 1;
maxJz = -jz;
switch((int)shpar[2])
{
case 1:
for(int x=-jx; x<=jx+1; ++x)
{
real rx2 = twox_ * Fabs(x * dx.data[0] - xcm);
if (rx2 <= shpar[0])
{
for(int y=-jy; y<=jy+1; ++y)
{
real ry2 = y * dx.data[1] - ycm;
ry2 = ry2 * ry2;
for(int z=-jz; z<=jz+1; ++z)
{
real rz2 = z * dx.data[2] - zcm;
rz2 = rz2 * rz2;
if (ry2 + rz2 <= r2)
{
InternalMinMax(x, y, z);
}
}
}
}
}
break;
case 2:
for(int x=-jx; x<=jx+1; ++x)
{
real rx2 = x * dx.data[0] - xcm;
rx2 = rx2 * rx2;
for(int y=-jy; y<=jy+1; ++y)
{
real ry2 = twox_ * Fabs(y * dx.data[1] - ycm);
if (ry2 <= shpar[0])
{
for(int z=-jz; z<=jz+1; ++z)
{
real rz2 = z * dx.data[2] - zcm;
rz2 = rz2 * rz2;
if (rx2 + rz2 <= r2)
{
InternalMinMax(x, y, z);
}
}
}
}
}
break;
case 3:
for(int x=-jx; x<=jx+1; ++x)
{
real rx2 = x * dx.data[0] - xcm;
rx2 = rx2 * rx2;
for(int y=-jy; y<=jy+1; ++y)
{
real ry2 = y * dx.data[1] - ycm;
ry2 = ry2 * ry2;
if (rx2 + ry2 <= r2)
{
for(int z=-jz; z<=jz+1; ++z)
{
real rz2 = twox_ * Fabs(z * dx.data[2] - zcm);
if (rz2 <= shpar[0])
{
InternalMinMax(x, y, z);
}
}
}
}
}
break;
default:
break;
}
}
/*
* Gauss-Sidel smoother for vector:
*
* As GS:
* 1. exchange off proc data
* 2. smooth local dof
*
* Assumption:
* 1. unknow dof is vector of dim 3
* 2. Matrix block for vector is
* | a1 -b 0 |
* | b a2 0 |
* | 0 0 a3 |
*
* */
void
mg_GaussSidel_vec(MAT *A, VEC *x, VEC *b, int nsmooth, void *ctx)
{
INT i, j, k, l, n, *pc, *pc0, *pc_offp, nlocal;
FLOAT *pd, *pd0, *pd_offp, *vx, *vb;
size_t *ps;
FLOAT sum[Dim], omega = _p->smooth_damp;
#if USE_MPI
FLOAT *offp_data = NULL;
#endif /* USE_MPI */
MagicCheck(VEC, x);
MagicCheck(VEC, b);
assert(x == NULL || x->nvec == 1);
assert(b == NULL || b->nvec == 1);
if (x != NULL && !x->assembled)
phgVecAssemble(x);
if (b != NULL && !b->assembled)
phgVecAssemble(b);
assert(A->type != PHG_DESTROYED);
if (!A->assembled)
phgMatAssemble(A);
assert(A->type != PHG_MATRIX_FREE);
phgMatPack(A);
#if USE_MPI
if (A->cmap->nprocs > 1) {
offp_data = phgAlloc(A->cinfo->rsize * sizeof(*offp_data));
}
#endif /* USE_MPI */
if (A->cmap->nlocal != A->rmap->nlocal ||
A->cmap->nlocal != x->map->nlocal ||
A->cmap->nlocal != b->map->nlocal)
phgError(1, "%s:%d: inconsistent matrix-vector.", __FILE__,
__LINE__);
if (A->cmap->nlocal % Dim != 0)
phgError(1, "%s: assume vector dof of dim 3!\n", __FUNCTION__);
#if USE_MPI
if (A->cmap->nprocs > 1) {
phgMapScatterBegin(A->cinfo, x->nvec, x->data, offp_data);
phgMapScatterEnd(A->cinfo, x->nvec, x->data, offp_data);
}
#endif /* USE_MPI */
/* iteration */
for (l = 0; l < nsmooth; l++) {
INT i_start, i_add;
/* multiply with local data */
vx = x->data;
vb = b->data;
pc0 = A->packed_cols;
pd0 = A->packed_data;
ps = A->packed_ind;
nlocal = A->rmap->nlocal;
/*
* lexicographic order: low to high
* Note: low->high and high->low alternatively does not help.
* */
if (TRUE || l % 2 == 0) {
i_start = 0;
i_add = Dim;
} else {
i_start = nlocal - Dim;
i_add = -Dim;
}
//for (i = i_start; i < A->rmap->nlocal && i >= 0; i += i_add) {
for (i = 0; i < nlocal ; i += Dim) {
INT jcol;
FLOAT aa[Dim][Dim], det, dx[Dim];
memset(aa, 0, sizeof(aa));
sum[0] = vb[i ];
sum[1] = vb[i+1];
sum[2] = vb[i+2];
/* local data */
pc = pc0 + PACK_COL(ps, i);
pd = pd0 + PACK_DAT(ps, i);
for (k = 0; k < Dim; k++) {
if ((n = *(pc++)) != 0) {
for (j = 0; j < n; j++) {
jcol = *(pc++);
if (jcol < i || jcol > i+2) {
sum[k] -= *(pd++) * vx[jcol];
} else { /* offD, jcol = i,i+1,i+2 */
aa[k][jcol - i] = *(pd++);
}
}
}
}
/* remote data */
if (A->cmap->nprocs > 1) {
pc_offp = pc0 + PACK_COL_OFFP(ps, i, nlocal);
pd_offp = pd0 + PACK_DAT_OFFP(ps, i, nlocal);
for (k = 0; k < Dim; k++) {
if ((n = *(pc_offp++)) != 0) {
for (j = 0; j < n; j++) {
jcol = *(pc_offp++);
sum[k] -= *(pd_offp++) * offp_data[jcol];
}
}
}
}
/* solve */
det = (aa[0][0] * aa[1][1] - aa[0][1] * aa[1][0]);
assert(Fabs(det) > 1e-12);
det = 1. / det;
dx[0] = (aa[1][1] * sum[0] - aa[0][1] * sum[1]) * det - vx[i ];
dx[1] = (aa[0][0] * sum[1] - aa[1][0] * sum[1]) * det - vx[i+1];
dx[2] = (1./aa[2][2] * sum[2]) - vx[i+2];
vx[i ] += omega * dx[0];
vx[i+1] += omega * dx[1];
vx[i+2] += omega * dx[2];
}
#if USE_MPI
if (A->cmap->nprocs > 1) {
phgMapScatterBegin(A->cinfo, x->nvec, x->data, offp_data);
phgMapScatterEnd(A->cinfo, x->nvec, x->data, offp_data);
}
#endif /* USE_MPI */
}
#if USE_MPI
phgFree(offp_data);
#endif /* USE_MPI */
return;
}
//------------------------------------------------------------------------
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);
}
void Tarcyl::Allocator(void) // A=cylinder length/d, B=cylinder diameter/d
{
int jlo, jhi;
int curSize = nx * ny;
ixyz.Dimension(curSize, 3);
real r2 = (real)0.25 * shpar[1] * shpar[1];
//
switch((int)shpar[2])
{
case 1:
jlo = maxJx;
jhi = minJx;
nat0 = 0;
for(int jx=minJx; jx<=maxJx; ++jx)
{
real rx2 = twox_ * Fabs(jx * dx.data[0] - xcm);
if (rx2 <= shpar[0])
{
for(int jy=minJy; jy<=maxJy; ++jy)
{
real ry2 = jy * dx.data[1] - ycm;
ry2 = ry2 * ry2;
for(int jz=minJz; jz<=maxJz; ++jz)
{
real rz2 = jz * dx.data[2] - zcm;
rz2 = rz2 * rz2;
if (ry2 + rz2 <= r2)
{
if (nat0 >= curSize)
{
ixyz.Extend(nz);
curSize = ixyz.GetSize(0);
}
ixyz.Fill3(nat0, jx, jy, jz);
int index = GetLinearAddress(nat0);
Composer(index, 0);
iocc[index] = true;
if (jx < jlo)
jlo = jx;
if (jx > jhi)
jhi = jx;
++nat0;
}
}
}
}
}
break;
case 2:
jlo = maxJy;
jhi = minJy;
nat0 = 0;
for(int jx=minJx; jx<=maxJx; ++jx)
{
real rx2 = jx * dx.data[0] - xcm;
rx2 = rx2 * rx2;
for(int jy=minJy; jy<=maxJy; ++jy)
{
real ry2 = twox_ * Fabs(jy * dx.data[1] - ycm);
if (ry2 <= shpar[0])
{
for(int jz=minJz; jz<=maxJz; ++jz)
{
real rz2 = jz * dx.data[2] - zcm;
rz2 = rz2 * rz2;
if (rx2 + rz2 <= r2)
{
if (nat0 >= curSize)
{
ixyz.Extend(nz);
curSize = ixyz.GetSize(0);
}
ixyz.Fill3(nat0, jx, jy, jz);
int index = GetLinearAddress(nat0);
Composer(index, 0);
iocc[index] = true;
if (jy < jlo)
jlo = jy;
if (jy > jhi)
jhi = jy;
++nat0;
}
}
}
}
}
break;
case 3:
jlo = maxJz;
jhi = minJz;
nat0 = 0;
for(int jx=minJx; jx<=maxJx; ++jx)
{
real rx2 = jx * dx.data[0] - xcm;
rx2 = rx2 * rx2;
for(int jy=minJy; jy<=maxJy; ++jy)
{
real ry2 = jy * dx.data[1] - ycm;
ry2 = ry2 * ry2;
if (rx2 + ry2 <= r2)
{
for(int jz=minJz; jz<=maxJz; ++jz)
{
real rz2 = twox_ * Fabs(jz * dx.data[2] - zcm);
if (rz2 <= shpar[0])
{
if (nat0 >= curSize)
{
ixyz.Extend(nz);
curSize = ixyz.GetSize(0);
}
ixyz.Fill3(nat0, jx, jy, jz);
int index = GetLinearAddress(nat0);
Composer(index, 0);
iocc[index] = true;
if (jz < jlo)
jlo = jz;
if (jz > jhi)
jhi = jz;
++nat0;
}
}
}
}
}
break;
default:
break;
}
int nlay = jhi - jlo + 1;
ixyz.Close(nat0);
//
// NLAY = number of layers in cylinder
// NFAC = number of atoms in slice
int nfac = nat0 / nlay;
//
// REFF2=effective radius**2 of disk
real reff2 = (real)nfac / Pi;
//
// ASPR=aspect ratio (length/diameter)
real aspr = half_ * (real)nlay / Sqrt(reff2);
//
// Description
sprintf(freeDescr, " Cyl.prism, NAT=%7d NFAC=%4d NLAY=%4d asp.ratio=%lf", nat0, nfac, nlay, aspr);
}
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);
}
void AbstractTarget::Dsyevj3(Matc3<real> &a, Matc3<real> &q, Vect3<real> &w)
{
/* **
----------------------------------------------------------------------
Numerical diagonalization of 3x3 matrcies
Copyright (C) 2006 Joachim Kopp
----------------------------------------------------------------------
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-13
----------------------------------------------------------------------
----------------------------------------------------------------------
Calculates the eigenvalues and normalized eigenvectors of a symmetric
matrix A using the Jacobi algorithm.
The upper triangular part of A is destroyed during the calculation,
the diagonal elements are read but not destroyed, and the lower
triangular elements are not referenced at all.
----------------------------------------------------------------------
Parameters:
A: The symmetric input matrix
Q: Storage buffer for eigenvectors
W: Storage buffer for eigenvalues
----------------------------------------------------------------------
C++ version was created by Choliy V., 2012.
** */
const int n = 3;
//
// Initialize Q to the identitity matrix
// --- This loop can be omitted if only the eigenvalues are desired -
q.Unity();
//
// Initialize W to diag(A)
int x, y;
for(x=0; x<n; ++x)
w.data[x] = a.Data(x, x);
//
// Calculate SQR(tr(A))
real sd = zero_;
for(x=0; x<n; ++x)
sd += Fabs(w.data[x]);
sd = sd*sd;
//
// Main iteration loop
int i;
for(i=0; i<50; ++i)
{
//
// Test for convergence
real so = zero_;
for(x=0; x<n; ++x)
for(y=x+1; y<n; ++y)
so += Fabs(a.Data(x, y));
if (so == zero_)
{
fprintf(stderr, "Dsyevj3 converged.\n");
return;
}
real thresh = (i < 3) ? (real)0.2*so/(n*n) : zero_;
//
// Do sweep
real t, theta;
for(x=0; x<n; ++x)
{
for(y=x+1; y<n; ++y)
{
real g = (real)100.0*(Fabs(a.Data(x,y)));
if ((i > 3) && (Fabs(w.data[x])+g == Fabs(w.data[x])) && (Fabs(w.data[y])+g == Fabs(w.data[y])))
a.Data(x,y) = zero_;
else
{
if (Fabs(a.Data(x,y)) > thresh) // Calculate Jacobi transformation
{
real h = w.data[y] - w.data[x];
if (Fabs(h)+g == Fabs(h))
t = a.Data(x,y) / h;
else
{
theta = half_*h/a.Data(x,y);
t = (theta < zero_) ? -(real)1./(Sqrt((real)1. + theta*theta) - theta) : (real)1./(sqrt((real)1. + theta*theta) + theta);
}
real c = (real)1./Sqrt((real)1. + t*t);
real s = t*c;
real z = t*a.Data(x,y);
// Apply Jacobi transformation
a.Data(x,y) = zero_;
w.data[x] -= z;
w.data[y] += z;
int r;
for(r=0; r<x; ++r)
{
t = a.Data(r, x);
a.Data(r, x) = c*t - s*a.Data(r, y);
a.Data(r, y) = s*t + c*a.Data(r, y);
}
for(r=x+1; r<y; ++r)
{
t = a.Data(x, r);
a.Data(x, r) = c*t - s*a.Data(r, y);
a.Data(r, y) = s*t + c*a.Data(r, y);
}
for(r=y+1; r<n; ++r)
{
t = a.Data(x, r);
a.Data(x, r) = c*t - s*a.Data(y, r);
a.Data(y, r) = s*t + c*a.Data(y, r);
}
// Update eigenvectors
// --- This loop can be omitted if only the eigenvalues are desired
for(r=0; r<n; ++r)
{
t = q.Data(r,x);
q.Data(r,x) = c*t - s*q.Data(r,y);
q.Data(r,y) = s*t + c*q.Data(r,y);
}
}
}
}
}
}
fprintf(stderr, ">DSYEVJ3: No convergence.\n");
}
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);
}
