Bounds bezier_bounds(const Vector2D& origin, const Matrix2D& m, const ControlPoint& p1, const ControlPoint& p2) {
assert(p1.segment_after == SEGMENT_CURVE);
// Transform the control points
Vector2D r1 = origin + p1.pos * m;
Vector2D r2 = origin + (p1.pos + p1.delta_after) * m;
Vector2D r3 = origin + (p2.pos + p2.delta_before) * m;
Vector2D r4 = origin + p2.pos * m;
// First of all, the corners should be in the bounding box
Bounds bounds(r1);
bounds.update(r4);
// Solve the derivative of the bezier curve to find its extremes
// It's only a quadtratic equation :)
BezierCurve curve(r1,r2,r3,r4);
double roots[4];
UInt count;
count = solve_quadratic(3*curve.a.x, 2*curve.b.x, curve.c.x, roots);
count += solve_quadratic(3*curve.a.y, 2*curve.b.y, curve.c.y, roots + count);
// now check them for min/max
for (UInt i = 0 ; i < count ; ++i) {
double t = roots[i];
if (t >=0 && t <= 1) {
bounds.update(curve.pointAt(t));
}
}
return bounds;
}
int main() {
std::vector<double> value;
try {
value = solve_quadratic(1, 2, 3);
std::cout << value[0] << ", " << value[1] << "\n";
} catch(Negative_delta const & e) {
std::cout << "negative delta value" << "\n";
}
try {
value = solve_quadratic(3, 20, 3);
std::cout << value[0] << ", " << value[1] << "\n";
} catch(Negative_delta const & e) {
std::cout << "negative delta value" << "\n";
}
std::cout << "second one" << "\n";
try {
value = solve_quadratic(1, 2, 3);
std::cout << value[0] << ", " << value[1] << "\n";
value = solve_quadratic(3, 20, 3);
std::cout << value[0] << ", " << value[1] << "\n";
} catch(Negative_delta const & e) {
std::cout << "negative delta value" << "\n";
}
}
bool CylinderShape::solve_hit_t(const Ray& ray, float& out_t_hit) const {
const auto& o = ray.orig.v;
const auto& d = ray.dir.v;
float a = square(d.x) + square(d.y);
float b = 2.f * (o.x * d.x + o.y * d.y);
float c = square(o.x) + square(o.y) - square(this->radius);
float t0, t1;
if(!solve_quadratic(a, b, c, t0, t1)) {
return false;
}
if(t1 < t0) {
std::swap(t0, t1);
}
if(ray.t_min <= t0 && t0 <= ray.t_max) {
float z0 = o.z + d.z * t0;
if(this->z_min <= z0 && z0 <= this->z_max) {
out_t_hit = t0;
return true;
}
}
if(ray.t_min <= t1 && t1 <= ray.t_max) {
float z1 = o.z + d.z * t1;
if(this->z_min <= z1 && z1 <= this->z_max) {
out_t_hit = t1;
return true;
}
}
return false;
}
UInt solve_cubic(double a, double b, double c, double d, double* roots) {
if (a == 0) {
return solve_quadratic(b, c, d, roots);
} else {
return solve_cubic(b/a, c/a, d/a, roots);
}
}
t_bool cylinder_intersection(t_ray *r, t_sphere *s, double *t0, double *t1)
{
t_vec3f diff;
t_vec3f dir;
t_f64 b;
t_f64 c;
t_f64 a;
t_f64 y0;
t_f64 y1;
dir = r->dir;
diff = sub_vec3f(&r->origin, &s->pos);
diff.y = 0;
dir.y = 0;
a = dot_vec3f(&dir, &dir);
b = 2 * (dot_vec3f(&diff, &dir));
c = dot_vec3f(&diff, &diff) - pow(s->radius, 2);
if (!solve_quadratic(dot_vec3f(&dir, &dir), b, c, t0, t1))
return (FALSE);
y0 = fabs(r->origin.y + r->dir.y * (*t0));
y1 = fabs(r->origin.y + r->dir.y * (*t1));
if (((y0 > (s->pos.y + 4)) && (y1 > (4 + s->pos.y))) ||
((y0 < s->pos.y) && (y1 < s->pos.y)))
return (FALSE);
return (TRUE);
}
bool az_arc_ray_hits_line(
az_vector_t p1, az_vector_t p2,
az_vector_t start, az_vector_t spin_center, double spin_angle,
double *angle_out, az_vector_t *point_out, az_vector_t *normal_out) {
// Calculate start, and the line, relative to spin_center. We will use
// parametric form for the line; it is the locus of points p0 + t * delta for
// all values of t.
const az_vector_t p0 = az_vsub(p1, spin_center);
const az_vector_t delta = az_vsub(p2, p1);
const az_vector_t rel_start = az_vsub(start, spin_center);
// Next we need to calculate the two points on the circle in which the ray
// travels that intersect the line. Derivation:
// Let sr = spin_radius (that is, vnorm(rel_start))
// (x0, y0) = p0
// (dx, dy) = delta
// Given a point on the line determined by t, we set the distance between
// that point and the origin (i.e. spin_center) equal to the spin_radius.
// Then we solve for t:
// sr^2 = (x0 + dx*t)^2 + (y0 + dy*t)^2
// sr^2 = x0^2 + 2*x0*dx*t + dx^2*t^2 + y0^2 + 2*y0*dy*t + dy^2*t^2
// 0 = (dx^2 + dy^2)*t^2 + (2*x0*dx + 2*y0*dy)*t + x0^2 + y0^2 - sr^2
// This is a simple quadratic equation. If there is no solution, that
// means that the ray can never hit the line, and we are done.
double t1, t2;
if (!solve_quadratic(az_vdot(delta, delta), 2.0 * az_vdot(p0, delta),
az_vdot(p0, p0) - az_vdot(rel_start, rel_start),
&t1, &t2)) return false;
// We now know the two points on the line that intersect the ray's circle.
// Next we find the ray angles that correspond to those two points:
const double start_theta = az_vtheta(rel_start);
const double angle1 =
az_vtheta(az_vadd(p0, az_vmul(delta, t1))) - start_theta;
const double angle2 =
az_vtheta(az_vadd(p0, az_vmul(delta, t2))) - start_theta;
// Pick the first angle at which we hit the circle (which depends on the sign
// of spin_angle):
const double angle =
(spin_angle > 0.0 ?
fmin(az_mod2pi_nonneg(angle1), az_mod2pi_nonneg(angle2)) :
fmax(az_mod2pi_nonpos(angle1), az_mod2pi_nonpos(angle2)));
// If the angle is within spin_angle (note that the two will have the same
// sign), then we hit the circle, otherwise we don't.
if (fabs(angle) > fabs(spin_angle)) return false;
if (angle_out != NULL) *angle_out = angle;
if (point_out != NULL) {
*point_out = az_vadd(spin_center,
az_vrotate(az_vsub(start, spin_center), angle));
}
if (normal_out != NULL) {
*normal_out = az_vflatten(az_vsub(start, p1), delta);
}
return true;
}
// Cubic solver
int solve_cubic(double c[4], double x0[3])
{
int num_roots;
if (is_zero(c[3])) return solve_quadratic(c, x0);
// normalize to x^3 + Ax^2 + Bx + C = 0
const double c3inv = 1.0/c[3];
const double A = c[2] * c3inv;
const double B = c[1] * c3inv;
const double C = c[0] * c3inv;
// substitute x = t - A/3 so t^3 + pt + q = 0
const double A2 = A*A;
const double p = (1.0/3.0)*((-1.0/3.0)*A2 + B);
const double q = 0.5*((2.0/27.0)*A*A2 - (1.0/3.0)*A*B + C);
// Cardano's fomula
const double p3 = p*p*p;
const double D = q*q + p3;
if (is_zero(D)) {
if (is_zero(q)) { // one triple soln
x0[0] = 0.0;
num_roots = 1;
}
else { // one single and one double soln
const double u = pow(fabs(q), 1.0/3.0)*sign(q);
x0[0] = -2.0*u;
x0[1] = u;
num_roots = 2;
}
}
else {
if (D < 0.0) { // three real roots
const double phi = 1.0/3.0 * acos(-q/sqrt(-p3));
const double t = 2.0 * sqrt(-p);
x0[0] = t * cos(phi);
x0[1] = -t * cos(phi + PI_OVER_3);
x0[2] = -t * cos(phi - PI_OVER_3);
num_roots = 3;
}
else { // one real root
const double sqrt_D = sqrt(D);
const double u = pow(sqrt_D + fabs(q), 1.0/3.0);
if (q > 0) x0[0] = -u + p / u;
else x0[0] = u - p / u;
num_roots = 1;
}
}
double sub = (1.0/3.0)*A;
for (int i=0; i<num_roots; i++) x0[i] -= sub;
return num_roots;
}
bool az_lead_target(az_vector_t rel_position, az_vector_t rel_velocity,
double proj_speed, az_vector_t *rel_impact_out) {
assert(proj_speed > 0.0);
double t1, t2;
if (!solve_quadratic(
az_vdot(rel_velocity, rel_velocity) - proj_speed * proj_speed,
2.0 * az_vdot(rel_position, rel_velocity),
az_vdot(rel_position, rel_position), &t1, &t2)) return false;
if (t1 < 0.0 && t2 < 0.0) return false;
if (rel_impact_out != NULL) {
const double t = (0.0 <= t1 && (t1 <= t2 || t2 < 0.0) ? t1 : t2);
*rel_impact_out = az_vadd(rel_position, az_vmul(rel_velocity, t));
}
return true;
}
void EQ::recalculate_band_coefficients() {
#define BAND_LOG(m_f) (log((m_f)) / log(2.))
for (int i = 0; i < band.size(); i++) {
double octave_size;
double frq = band[i].freq;
if (i == 0) {
octave_size = BAND_LOG(band[1].freq) - BAND_LOG(frq);
} else if (i == (band.size() - 1)) {
octave_size = BAND_LOG(frq) - BAND_LOG(band[i - 1].freq);
} else {
double next = BAND_LOG(band[i + 1].freq) - BAND_LOG(frq);
double prev = BAND_LOG(frq) - BAND_LOG(band[i - 1].freq);
octave_size = (next + prev) / 2.0;
}
double frq_l = round(frq / pow(2.0, octave_size / 2.0));
double side_gain2 = POW2(Math_SQRT12);
double th = 2.0 * Math_PI * frq / mix_rate;
double th_l = 2.0 * Math_PI * frq_l / mix_rate;
double c2a = side_gain2 * POW2(cos(th)) - 2.0 * side_gain2 * cos(th_l) * cos(th) + side_gain2 - POW2(sin(th_l));
double c2b = 2.0 * side_gain2 * POW2(cos(th_l)) + side_gain2 * POW2(cos(th)) - 2.0 * side_gain2 * cos(th_l) * cos(th) - side_gain2 + POW2(sin(th_l));
double c2c = 0.25 * side_gain2 * POW2(cos(th)) - 0.5 * side_gain2 * cos(th_l) * cos(th) + 0.25 * side_gain2 - 0.25 * POW2(sin(th_l));
//printf("band %i, precoefs = %f,%f,%f\n",i,c2a,c2b,c2c);
double r1, r2; //roots
int roots = solve_quadratic(c2a, c2b, c2c, &r1, &r2);
ERR_CONTINUE(roots == 0);
band[i].c1 = 2.0 * ((0.5 - r1) / 2.0);
band[i].c2 = 2.0 * r1;
band[i].c3 = 2.0 * (0.5 + r1) * cos(th);
//printf("band %i, coefs = %f,%f,%f\n",i,(float)bands[i].c1,(float)bands[i].c2,(float)bands[i].c3);
}
}
static int intersect_cylinder(t_vector ray, t_object *cylinder,
t_double2 *distance)
{
double a;
double b;
double c;
a = ray.dir.x * ray.dir.x + ray.dir.y * ray.dir.y;
b = 2 * (ray.pos.x * ray.dir.x + ray.pos.y *
ray.dir.y);
c = ray.pos.x * ray.pos.x + ray.pos.y * ray.pos.y -
cylinder->radius * cylinder->radius;
if (solve_quadratic(a, b, c, distance))
return (1);
return (0);
}
/**
* Calculates whether a ray intersects with a sphere. Returns true or false,
* storing the collision point in result if true.
*/
bool Sphere::intersection(Ray prim_ray, Vec3 &result) {
double t0, t1;
Vec3 obj_center(pos);
Vec3 L = (*prim_ray.p1) - obj_center;
double a = prim_ray.p2->dot(*prim_ray.p2);
double b = 2 * prim_ray.p2->dot(L);
double c = L.dot(L) - (radius * radius);
if(!solve_quadratic(a, b, c, t0, t1)) {
return false;
} else {
prim_ray.eval(t0, result);
return true;
}
}
int main(int argc, char **argv)
{
int a = 1;
int b = 2;
int c = 3;
double answer;
if (check_discriminant(a, b, c) == true)
{
solve_quadratic(a,b,c);
}
else
{
printf("Roots are imaginary.\n");
}
return 0;
}
// Like az_ray_hits_circle, but if the ray starts inside the circle, it isn't
// counted as an immediate impact.
static bool ray_hits_hollow_circle(
double radius, az_vector_t center, az_vector_t start, az_vector_t delta,
az_vector_t *pos_out) {
assert(radius >= 0.0);
// Set up a quadratic equation modeling when the ray will hit the circle
// (assuming it travels t*delta from start at time t). If there is no
// solution, the ray misses the circle; otherwise, we still need to check
// if the solution is in range.
const az_vector_t rel_start = az_vsub(start, center);
double t1, t2;
if (!solve_quadratic(az_vdot(delta, delta), 2.0 * az_vdot(rel_start, delta),
az_vdot(rel_start, rel_start) - radius * radius,
&t1, &t2)) return false;
const double t = (0.0 <= t1 && (t1 <= t2 || t2 < 0.0) ? t1 : t2);
if (0.0 <= t && t <= 1.0) {
if (pos_out != NULL) *pos_out = az_vadd(start, az_vmul(delta, t));
return true;
} else return false;
}
s_res ellipse_dst(s_geo sp, s_cam cam, s_res prev)
{
vec3 dir;
vec3 centre;
s_res ret;
centre = (cam.pos - sp.pos) / sp.a.xyz;
dir = cam.ray / sp.a.xyz;
if ((ret.dst = solve_quadratic(dot(dir, dir),
dot(dir, centre) * 2, dot(centre, centre) - 1)) == -1)
return (prev);
if (ret.dst > 0 && (prev.dst <= 0 || ret.dst < prev.dst))
{
ret.mat = sp.mat;
ret.cam = cam;
ret.normal = ellipse_norm(cam, ret, sp);
return (ret);
}
return (prev);
}
bool SphereShape::solve_hit_t(const Ray& ray, float& out_t_hit) const {
float a = dot(ray.dir.v, ray.dir.v);
float b = 2.f * dot(ray.orig.v, ray.dir.v);
float c = dot(ray.orig.v, ray.orig.v) - this->radius * this->radius;
float t0, t1;
if(!solve_quadratic(a, b, c, t0, t1)) {
return false;
}
if(t1 < t0) {
std::swap(t0, t1);
}
if(ray.t_min <= t0 && t0 <= ray.t_max) {
out_t_hit = t0;
} else if(ray.t_min <= t1 && t1 <= ray.t_max) {
out_t_hit = t1;
} else {
return false;
}
return true;
}
int solve_cubic(t_sol obj, double *results)
{
double var[13];
var[12] = obj.coef[3];
if (fabs(var[12]) < EPSILON)
return (solve_quadratic(obj, results));
else
{
if (!double_are_same(var[12], 1.0))
{
var[9] = obj.coef[2] / var[12];
var[10] = obj.coef[1] / var[12];
var[11] = obj.coef[0] / var[12];
}
else
{
var[9] = obj.coef[2];
var[10] = obj.coef[1];
var[11] = obj.coef[0];
}
}
return (solve_exp_tree(var, results));
}
int TrajectoryRep1D::solve( double K[3], double x, int extrapolate )
{
#ifdef DEBUG_TRAJECTORY
std::cout << "solve( x = " << x << " ):\n";
switch( _rep ) {
case TRAJ_EMPTY:
std::cout << " rep = TRAJ_EMPTY\n";
break;
case TRAJ_LINEAR:
std::cout << " rep = TRAJ_LINEAR\n";
break;
case TRAJ_QUADRATIC:
std::cout << " rep = TRAJ_QUADRATIC\n";
break;
case TRAJ_CUBIC:
std::cout << " rep = TRAJ_CUBIC\n";
break;
};
#endif
switch( _rep ) {
case TRAJ_EMPTY:
throw( Error( ERROR_LOCATION, "empty representation" ) );
break;
case TRAJ_LINEAR:
{
if( _A == 0.0 )
return( 0 );
// Some tests fail here of spotting K equal to one
// Rounding to 64-bit from 80-bit help
// This is really not the way to go... temporary
volatile double KT = (x-_B) / _A;
K[0] = KT;
if( in( K[0], extrapolate ) )
return( 1 );
break;
}
case TRAJ_QUADRATIC:
{
int nroots = solve_quadratic( _A, _B, _C-x, &K[0], &K[1] );
#ifdef DEBUG_TRAJECTORY
std::cout << " nroots = " << nroots << "\n";
for( int a = 0; a < nroots; a++ )
std::cout << " K[" << a << "] = " << K[a] <<"\n";
#endif
if( nroots == 0 ) {
return( 0 );
} else if( nroots == 1 ) {
if( in( K[0], extrapolate ) ) {
return( 1 );
} else {
return( 0 );
}
} else /* nroots = 2 */ {
if( in( K[0], extrapolate ) ) {
if( in( K[1], extrapolate ) ) {
return( 2 );
} else {
return( 1 );
}
} else {
if( in( K[1], extrapolate ) ) {
K[0] = K[1];
return( 1 );
} else {
return( 0 );
}
}
}
break;
}
case TRAJ_CUBIC:
{
int nroots = solve_cubic( _A, _B, _C, _D-x, &K[0], &K[1], &K[2] );
#ifdef DEBUG_TRAJECTORY
std::cout << " nroots = " << nroots << "\n";
for( int a = 0; a < nroots; a++ )
std::cout << " K[" << a << "] = " << K[a] <<"\n";
#endif
if( nroots == 0 ) {
return( 0 );
} else if( nroots == 1 ) {
if( in( K[0], extrapolate ) ) {
return( 1 );
}
} else if( nroots == 2 ) {
if( in( K[0], extrapolate ) ) {
if( in( K[1], extrapolate ) ) {
return( 2 );
} else {
return( 1 );
}
} else {
if( in( K[1], extrapolate ) ) {
K[0] = K[1];
return( 1 );
} else {
return( 0 );
}
}
} else /* nroots = 3 */ {
if( in( K[0], extrapolate ) ) {
if( in( K[1], extrapolate ) ) {
if( in( K[2], extrapolate ) ) {
return( 3 );
} else {
return( 2 );
}
} else {
if( in( K[2], extrapolate ) ) {
K[1] = K[2];
return( 2 );
} else {
return( 1 );
}
}
} else {
if( in( K[1], extrapolate ) ) {
if( in( K[2], extrapolate ) ) {
K[0] = K[1];
K[1] = K[2];
return( 2 );
} else {
K[0] = K[1];
return( 1 );
}
} else {
if( in( K[2], extrapolate ) ) {
K[0] = K[2];
return( 1 );
} else {
return( 0 );
}
}
}
}
break;
}
};
return( 0 );
}
bool
RestoRestorationPhase::PerformRestoration()
{
DBG_START_METH("RestoRestorationPhase::PerformRestoration",
dbg_verbosity);
Jnlst().Printf(J_DETAILED, J_MAIN,
"Performing second level restoration phase for current constriant violation %8.2e\n", IpCq().curr_constraint_violation());
DBG_ASSERT(IpCq().curr_constraint_violation()>0.);
// Get a grip on the restoration phase NLP and obtain the pointers
// to the original NLP data
SmartPtr<RestoIpoptNLP> resto_ip_nlp =
static_cast<RestoIpoptNLP*> (&IpNLP());
DBG_ASSERT(dynamic_cast<RestoIpoptNLP*> (&IpNLP()));
SmartPtr<IpoptNLP> orig_ip_nlp =
static_cast<IpoptNLP*> (&resto_ip_nlp->OrigIpNLP());
DBG_ASSERT(dynamic_cast<IpoptNLP*> (&resto_ip_nlp->OrigIpNLP()));
// Get the current point and create a new vector for the result
SmartPtr<const CompoundVector> Ccurr_x =
static_cast<const CompoundVector*> (GetRawPtr(IpData().curr()->x()));
SmartPtr<Vector> new_x = IpData().curr()->x()->MakeNew();
SmartPtr<CompoundVector> Cnew_x =
static_cast<CompoundVector*> (GetRawPtr(new_x));
// The x values remain unchanged
SmartPtr<Vector> x = Cnew_x->GetCompNonConst(0);
x->Copy(*Ccurr_x->GetComp(0));
// ToDo in free mu mode - what to do here?
Number mu = IpData().curr_mu();
// Compute the initial values for the n and p variables for the
// equality constraints
Number rho = resto_ip_nlp->Rho();
SmartPtr<Vector> nc = Cnew_x->GetCompNonConst(1);
SmartPtr<Vector> pc = Cnew_x->GetCompNonConst(2);
SmartPtr<const Vector> cvec = orig_ip_nlp->c(*Ccurr_x->GetComp(0));
SmartPtr<Vector> a = nc->MakeNew();
SmartPtr<Vector> b = nc->MakeNew();
a->Set(mu/(2.*rho));
a->Axpy(-0.5, *cvec);
b->Copy(*cvec);
b->Scal(mu/(2.*rho));
solve_quadratic(*a, *b, *nc);
pc->Copy(*cvec);
pc->Axpy(1., *nc);
DBG_PRINT_VECTOR(2, "nc", *nc);
DBG_PRINT_VECTOR(2, "pc", *pc);
// initial values for the n and p variables for the inequality
// constraints
SmartPtr<Vector> nd = Cnew_x->GetCompNonConst(3);
SmartPtr<Vector> pd = Cnew_x->GetCompNonConst(4);
SmartPtr<Vector> dvec = pd->MakeNew();
dvec->Copy(*orig_ip_nlp->d(*Ccurr_x->GetComp(0)));
dvec->Axpy(-1., *IpData().curr()->s());
a = nd->MakeNew();
b = nd->MakeNew();
a->Set(mu/(2.*rho));
a->Axpy(-0.5, *dvec);
b->Copy(*dvec);
b->Scal(mu/(2.*rho));
solve_quadratic(*a, *b, *nd);
pd->Copy(*dvec);
pd->Axpy(1., *nd);
DBG_PRINT_VECTOR(2, "nd", *nd);
DBG_PRINT_VECTOR(2, "pd", *pd);
// Now set the trial point to the solution of the restoration phase
// s and all multipliers remain unchanged
SmartPtr<IteratesVector> new_trial = IpData().curr()->MakeNewContainer();
new_trial->Set_x(*new_x);
IpData().set_trial(new_trial);
IpData().Append_info_string("R");
return true;
}
static int solve_cubic(DBL *x, DBL *y)
{
DBL Q, R, Q3, R2, sQ, d, an, theta;
DBL A2, a0, a1, a2, a3;
a0 = x[0];
if (a0 == 0.0)
{
return(solve_quadratic(&x[1], y));
}
else
{
if (a0 != 1.0)
{
a1 = x[1] / a0;
a2 = x[2] / a0;
a3 = x[3] / a0;
}
else
{
a1 = x[1];
a2 = x[2];
a3 = x[3];
}
}
A2 = a1 * a1;
Q = (A2 - 3.0 * a2) / 9.0;
/* Modified to save some multiplications and to avoid a floating point
exception that occured with DJGPP and full optimization. [DB 8/94] */
R = (a1 * (A2 - 4.5 * a2) + 13.5 * a3) / 27.0;
Q3 = Q * Q * Q;
R2 = R * R;
d = Q3 - R2;
an = a1 / 3.0;
if (d >= 0.0)
{
/* Three real roots. */
d = R / sqrt(Q3);
theta = acos(d) / 3.0;
sQ = -2.0 * sqrt(Q);
y[0] = sQ * cos(theta) - an;
y[1] = sQ * cos(theta + TWO_M_PI_3) - an;
y[2] = sQ * cos(theta + FOUR_M_PI_3) - an;
return(3);
}
else
{
sQ = pow(sqrt(R2 - Q3) + fabs(R), 1.0 / 3.0);
if (R < 0)
{
y[0] = (sQ + Q / sQ) - an;
}
else
{
y[0] = -(sQ + Q / sQ) - an;
}
return(1);
}
}
int Solve_Polynomial(int n, DBL *c0, DBL *r, int sturm, DBL epsilon)
{
int roots, i;
DBL *c;
Increase_Counter(stats[Polynomials_Tested]);
roots = 0;
/*
* Determine the "real" order of the polynomial, i.e.
* eliminate small leading coefficients.
*/
i = 0;
while ((fabs(c0[i]) < SMALL_ENOUGH) && (i < n))
{
i++;
}
n -= i;
c = &c0[i];
switch (n)
{
case 0:
break;
case 1:
/* Solve linear polynomial. */
if (c[0] != 0.0)
{
r[roots++] = -c[1] / c[0];
}
break;
case 2:
/* Solve quadratic polynomial. */
roots = solve_quadratic(c, r);
break;
case 3:
/* Root elimination? */
if (epsilon > 0.0)
{
if ((c[2] != 0.0) && (fabs(c[3]/c[2]) < epsilon))
{
Increase_Counter(stats[Roots_Eliminated]);
roots = solve_quadratic(c, r);
break;
}
}
/* Solve cubic polynomial. */
if (sturm)
{
roots = polysolve(3, c, r);
}
else
{
roots = solve_cubic(c, r);
}
break;
case 4:
/* Root elimination? */
if (epsilon > 0.0)
{
if ((c[3] != 0.0) && (fabs(c[4]/c[3]) < epsilon))
{
Increase_Counter(stats[Roots_Eliminated]);
if (sturm)
{
roots = polysolve(3, c, r);
}
else
{
roots = solve_cubic(c, r);
}
break;
}
}
/* Test for difficult coeffs. */
if (difficult_coeffs(4, c))
{
sturm = true;
}
/* Solve quartic polynomial. */
if (sturm)
{
roots = polysolve(4, c, r);
}
else
{
roots = solve_quartic(c, r);
}
break;
default:
if (epsilon > 0.0)
{
if ((c[n-1] != 0.0) && (fabs(c[n]/c[n-1]) < epsilon))
{
Increase_Counter(stats[Roots_Eliminated]);
roots = polysolve(n-1, c, r);
}
}
/* Solve n-th order polynomial. */
roots = polysolve(n, c, r);
break;
}
return(roots);
}
bool RestoIterateInitializer::SetInitialIterates()
{
DBG_START_METH("RestoIterateInitializer::SetInitialIterates",
dbg_verbosity);
// Get a grip on the restoration phase NLP and obtain the pointers
// to the original NLP data
SmartPtr<RestoIpoptNLP> resto_ip_nlp =
static_cast<RestoIpoptNLP*> (&IpNLP());
SmartPtr<IpoptNLP> orig_ip_nlp =
static_cast<IpoptNLP*> (&resto_ip_nlp->OrigIpNLP());
SmartPtr<IpoptData> orig_ip_data =
static_cast<IpoptData*> (&resto_ip_nlp->OrigIpData());
SmartPtr<IpoptCalculatedQuantities> orig_ip_cq =
static_cast<IpoptCalculatedQuantities*> (&resto_ip_nlp->OrigIpCq());
// Set the value of the barrier parameter
Number resto_mu;
resto_mu = Max(orig_ip_data->curr_mu(),
orig_ip_cq->curr_c()->Amax(),
orig_ip_cq->curr_d_minus_s()->Amax());
IpData().Set_mu(resto_mu);
Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
"Initial barrier parameter resto_mu = %e\n", resto_mu);
/////////////////////////////////////////////////////////////////////
// Initialize primal varialbes //
/////////////////////////////////////////////////////////////////////
// initialize the data structures in the restoration phase NLP
IpData().InitializeDataStructures(IpNLP(), false, false, false,
false, false);
SmartPtr<Vector> new_x = IpData().curr()->x()->MakeNew();
SmartPtr<CompoundVector> Cnew_x =
static_cast<CompoundVector*> (GetRawPtr(new_x));
// Set the trial x variables from the original NLP
Cnew_x->GetCompNonConst(0)->Copy(*orig_ip_data->curr()->x());
// Compute the initial values for the n and p variables for the
// equality constraints
Number rho = resto_ip_nlp->Rho();
DBG_PRINT((1,"rho = %e\n", rho));
SmartPtr<Vector> nc = Cnew_x->GetCompNonConst(1);
SmartPtr<Vector> pc = Cnew_x->GetCompNonConst(2);
SmartPtr<const Vector> cvec = orig_ip_cq->curr_c();
DBG_PRINT_VECTOR(2, "cvec", *cvec);
SmartPtr<Vector> a = nc->MakeNew();
SmartPtr<Vector> b = nc->MakeNew();
a->Set(resto_mu/(2.*rho));
a->Axpy(-0.5, *cvec);
b->Copy(*cvec);
b->Scal(resto_mu/(2.*rho));
DBG_PRINT_VECTOR(2, "a", *a);
DBG_PRINT_VECTOR(2, "b", *b);
solve_quadratic(*a, *b, *nc);
pc->Copy(*cvec);
pc->Axpy(1., *nc);
DBG_PRINT_VECTOR(2, "nc", *nc);
DBG_PRINT_VECTOR(2, "pc", *pc);
// initial values for the n and p variables for the inequality
// constraints
SmartPtr<Vector> nd = Cnew_x->GetCompNonConst(3);
SmartPtr<Vector> pd = Cnew_x->GetCompNonConst(4);
cvec = orig_ip_cq->curr_d_minus_s();
a = nd->MakeNew();
b = nd->MakeNew();
a->Set(resto_mu/(2.*rho));
a->Axpy(-0.5, *cvec);
b->Copy(*cvec);
b->Scal(resto_mu/(2.*rho));
solve_quadratic(*a, *b, *nd);
pd->Copy(*cvec);
pd->Axpy(1., *nd);
DBG_PRINT_VECTOR(2, "nd", *nd);
DBG_PRINT_VECTOR(2, "pd", *pd);
// Leave the slacks unchanged
SmartPtr<const Vector> new_s = orig_ip_data->curr()->s();
// Now set the primal trial variables
DBG_PRINT_VECTOR(2,"new_s",*new_s);
DBG_PRINT_VECTOR(2,"new_x",*new_x);
SmartPtr<IteratesVector> trial = IpData().curr()->MakeNewContainer();
trial->Set_primal(*new_x, *new_s);
IpData().set_trial(trial);
DBG_PRINT_VECTOR(2, "resto_c", *IpCq().trial_c());
DBG_PRINT_VECTOR(2, "resto_d_minus_s", *IpCq().trial_d_minus_s());
/////////////////////////////////////////////////////////////////////
// Initialize bound multipliers //
/////////////////////////////////////////////////////////////////////
SmartPtr<Vector> new_z_L = IpData().curr()->z_L()->MakeNew();
SmartPtr<CompoundVector> Cnew_z_L =
static_cast<CompoundVector*> (GetRawPtr(new_z_L));
DBG_ASSERT(IsValid(Cnew_z_L));
SmartPtr<Vector> new_z_U = IpData().curr()->z_U()->MakeNew();
SmartPtr<Vector> new_v_L = IpData().curr()->v_L()->MakeNew();
SmartPtr<Vector> new_v_U = IpData().curr()->v_U()->MakeNew();
// multipliers for the original bounds are
SmartPtr<const Vector> orig_z_L = orig_ip_data->curr()->z_L();
SmartPtr<const Vector> orig_z_U = orig_ip_data->curr()->z_U();
SmartPtr<const Vector> orig_v_L = orig_ip_data->curr()->v_L();
SmartPtr<const Vector> orig_v_U = orig_ip_data->curr()->v_U();
// Set the new multipliers to the min of the penalty parameter Rho
// and their current value
SmartPtr<Vector> Cnew_z_L0 = Cnew_z_L->GetCompNonConst(0);
Cnew_z_L0->Set(rho);
Cnew_z_L0->ElementWiseMin(*orig_z_L);
new_z_U->Set(rho);
new_z_U->ElementWiseMin(*orig_z_U);
new_v_L->Set(rho);
new_v_L->ElementWiseMin(*orig_v_L);
new_v_U->Set(rho);
new_v_U->ElementWiseMin(*orig_v_U);
// Set the multipliers for the p and n bounds to the "primal" multipliers
SmartPtr<Vector> Cnew_z_L1 = Cnew_z_L->GetCompNonConst(1);
Cnew_z_L1->Set(resto_mu);
Cnew_z_L1->ElementWiseDivide(*nc);
SmartPtr<Vector> Cnew_z_L2 = Cnew_z_L->GetCompNonConst(2);
Cnew_z_L2->Set(resto_mu);
Cnew_z_L2->ElementWiseDivide(*pc);
SmartPtr<Vector> Cnew_z_L3 = Cnew_z_L->GetCompNonConst(3);
Cnew_z_L3->Set(resto_mu);
Cnew_z_L3->ElementWiseDivide(*nd);
SmartPtr<Vector> Cnew_z_L4 = Cnew_z_L->GetCompNonConst(4);
Cnew_z_L4->Set(resto_mu);
Cnew_z_L4->ElementWiseDivide(*pd);
// Set those initial values to be the trial values in Data
trial = IpData().trial()->MakeNewContainer();
trial->Set_bound_mult(*new_z_L, *new_z_U, *new_v_L, *new_v_U);
IpData().set_trial(trial);
/////////////////////////////////////////////////////////////////////
// Initialize equality constraint multipliers //
/////////////////////////////////////////////////////////////////////
DefaultIterateInitializer::least_square_mults(
Jnlst(), IpNLP(), IpData(), IpCq(),
resto_eq_mult_calculator_, constr_mult_init_max_);
// upgrade the trial to the current point
IpData().AcceptTrialPoint();
DBG_PRINT_VECTOR(2, "y_c", *IpData().curr()->y_c());
DBG_PRINT_VECTOR(2, "y_d", *IpData().curr()->y_d());
DBG_PRINT_VECTOR(2, "z_L", *IpData().curr()->z_L());
DBG_PRINT_VECTOR(2, "z_U", *IpData().curr()->z_U());
DBG_PRINT_VECTOR(2, "v_L", *IpData().curr()->v_L());
DBG_PRINT_VECTOR(2, "v_U", *IpData().curr()->v_U());
return true;
}
int solve_cubic ( float a, float b, float c, float d,
float* x1, float* x2, float* x3 )
{
/*
* 3 2
* Solve ax + bx + cx + d = 0
*
* Return values: 0 - no solution
* -1 - infinite number of solutions
* 1 - only one distinct root, real, possibly of multiplicity 2 or 3.
* returned in *x1
* 2 - only two distinct roots, both real, one possibly of multiplicity 2,
* returned in *x1 and *x2
* -2 - only two distinct roots (complex conjugates),
* real part returned in *x1,
* imaginary part returned in *x2
* 3 - three distinct real roots of multiplicity 1
* returned in *x1, *x2, and *x3.
* -3 - one real root and one complex conjugate pair.
* real root in *x1, real part of complex conjugate root
* in *x2, imaginary part in *x3.
*
* XXX - this whole scheme is wrong.
* Each root should be returned along with its multiplicity.
*/
double p, q, r, a2, b2, z1, z2, z3, des;
*x1 = *x2 = *x3 = 0.0;
if (a == 0.0)
return (solve_quadratic (b, c, d, x1, x2));
else
{
p = b/a; /* reduce to y^3 + py^2 + qy + r = 0 */
q = c/a; /* by dividing through by a */
r = d/a;
a2 = (3*q - p*p)/3; /* reduce to z^3 + a2*z + b2 = 0 */
Zero_test2 (a2, 3*q, p*p);
b2 = (2*p*p*p - 9*p*q + 27*r)/27; /* using y = z - p/3 */
Zero_test3 (b2, 2*p*p*p, 9*p*q, 27*r);
des = (b2*b2/4 + a2*a2*a2/27);
Zero_test2 (des, b2*b2/4, a2*a2*a2/27);
if (des == 0.0) /* three real roots, at least two equal */
{
double a3 = cube_root (-b2/2);
if (a3 == 0.0) /* one distinct real root of multiplicity three */
{
z1 = 0.0;
*x1 = z1 - p/3;
Zero_test2 (*x1, z1, p/3);
*x2 = 0.0;
*x3 = 0.0;
return (1);
}
else /* one real root multiplicity one, another of multiplicity two */
{
z1 = 2*a3;
z2 = -a3;
*x1 = z1 - p/3;
Zero_test2 (*x1, z1, p/3);
*x2 = z2 - p/3;
Zero_test2 (*x2, z2, p/3);
*x3 = 0.0;
return (2);
}
}
else if (des > 0.0) /* one real root, one complex conjugate pair */
{
double d2 = sqrt(des);
double t1 = -b2/2 + d2;
double t2 = -b2/2 - d2;
double a3;
double b3;
Zero_test2 (t1, b2/2, d2);
Zero_test2 (t2, b2/2, d2);
a3 = cube_root (t1);
b3 = cube_root (t2);
z1 = a3 + b3;
Zero_test2 (z1, a3, b3);
z2 = - z1/2;
t1 = a3-b3;
Zero_test2 (t1, a3, b3);
z3 = sqrt(3.0) * t1/2;
*x1 = z1 - p/3;
Zero_test2 (*x1, z1, p/3);
*x2 = z2 - p/3;
Zero_test2 (*x2, z2, p/3);
*x3 = z3;
return (-3);
}
else if (des < 0.0) /* three unequal real roots */
{
double temp_r, theta, cos_term, sin_term, t1;
t1 = b2*b2/4 - des;
Zero_test2 (t1, b2*b2/4, des);
temp_r = cube_root (sqrt (b2*b2/4 - des));
theta = atan2 (sqrt(-des), (-b2/2));
cos_term = temp_r * cos (theta/3);
sin_term = temp_r * sin (theta/3) * sqrt(3.0);
z1 = 2 * cos_term;
z2 = - cos_term - sin_term;
Zero_test2 (z2, cos_term, sin_term);
z3 = - cos_term + sin_term;
Zero_test2 (z3, cos_term, sin_term);
*x1 = z1 - p/3;
Zero_test2 (*x1, z1, p/3);
*x2 = z2 - p/3;
Zero_test2 (*x2, z2, p/3);
*x3 = z3 - p/3;
Zero_test2 (*x3, z3, p/3);
return (3);
}
else /* cannot happen */
{
fprintf (stderr, "impossible descriminant in solve_cubic\n");
return (0);
}
}
}
xcollision_agent* xmove::fall (float &t, xvec2f_t &speed, float accy, unsigned extra_filter, unsigned max_iteration)
{
m_kpos.clear();
if( m_terrain || speed.y>0 || (speed.y==0 && accy==0) ) {
return 0;
}
float tfall, tleft = 0;
if(accy>0)
{
tfall = -speed.y / accy;
if(tfall<t)
{
tleft = t-tfall;
t = tfall;
}
}
xvec2f_t pos = m_agent->get_position(), offset, collide_normal;
float dt;
xcollision_agent *collidee = 0;
unsigned group_filter = ms_standon_filter | extra_filter;
bool is_on_ground = false;
for(unsigned iteration = 0; iteration<max_iteration; ++iteration)
{
offset.x = speed.x * t;
offset.y = speed.y * t + 0.5f * accy * t * t;
collidee = _detect_collision(offset,group_filter,dt,collide_normal,ms_platform_filter);
if(collidee)
{ // hit something
offset.x *= dt;
offset.y *= dt;
pos += offset;
tfall = solve_quadratic(0.5f*accy, speed.y, -offset.y);
t -= tfall;
speed.y += accy * tfall;
if(collidee->get_mask() & ms_standon_filter)
{ // hit terrain
speed.x = 0;
if(collide_normal.y < EPSILON)
{
pos.x += ms_drawback * collide_normal.x;
pos.y += ms_drawback * collide_normal.y;
m_agent->set_position(pos.x, pos.y);
PUSH_KPOS_WITH_SPEED(m_kpos, tfall, pos.x, pos.y, speed.x, speed.y);
continue;
}
// fall on ground
pos.y += ms_drawback;
speed.y= 0;
is_on_ground = true;
}
}
else
{
speed.y += accy * t;
pos += offset;
tfall = t;
t = 0;
}
m_agent->set_position(pos.x,pos.y);
if(is_on_ground)
set_terrain(collidee);
PUSH_KPOS_WITH_SPEED(m_kpos, tfall, pos.x, pos.y, speed.x, speed.y);
break;
} // for
t += tleft;
m_agent->update_aabb();
return collidee;
}
void solve(T const & a , T const & b, T const & c) {
if (a == 0) { solve_linear(b, c); }
else { solve_quadratic(a, b, c); }
}
xcollision_agent* xmove::jump (float &t, xvec2f_t &speed, float accy, unsigned extra_filter, unsigned max_iteration)
{
m_kpos.clear();
if(speed.y<0 || (speed.y==0 && accy<=0))
return 0;
float tjump, tleft = 0;
if(accy<0)
{
tjump = -speed.y / accy;
if(tjump<t)
{
tleft = t-tjump;
t = tjump;
}
}
xvec2f_t offset, collide_normal;
xcollision_agent *collidee = 0;
float dt;
unsigned group_filter = ms_terrain_filter | extra_filter;
// leave ground
m_terrain = 0;
for(unsigned iteration = 0; iteration<max_iteration; ++iteration)
{
offset.x = speed.x * t;
offset.y = speed.y * t + 0.5f * accy * t * t;
collidee = _detect_collision(offset, group_filter, dt, collide_normal);
xvec2f_t pos = m_agent->get_position();
if(collidee)
{
offset *= dt;
pos += offset;
tjump = solve_quadratic(0.5f*accy, speed.y, -offset.y);
t -= tjump;
if(!(collidee->get_mask() & extra_filter))
{
pos.x += collide_normal.x * ms_drawback;
pos.y += collide_normal.y * ms_drawback;
if(std::fabs(collide_normal.x)>EPSILON)
speed.x = 0;
if(std::fabs(collide_normal.y)<EPSILON){
speed.y += tjump*accy;
m_agent->set_position(pos.x, pos.y);
PUSH_KPOS_WITH_SPEED(m_kpos, tjump, pos.x, pos.y, speed.x, speed.y);
continue;
}
speed.y = 0;
}
}
else
{
pos += offset;
speed.y = tleft>0 ? 0 : speed.y+accy*t;
tjump = t;
t = 0;
}
m_agent->set_position(pos.x, pos.y);
PUSH_KPOS_WITH_SPEED(m_kpos, tjump, pos.x, pos.y, speed.x, speed.y);
break;
} // for
t += tleft;
m_agent->update_aabb();
return collidee;
}
