19e49fb63d355446b91d20ff78ad78b297e89a50dcaryclark@google.com/* 29e49fb63d355446b91d20ff78ad78b297e89a50dcaryclark@google.com * Copyright 2012 Google Inc. 39e49fb63d355446b91d20ff78ad78b297e89a50dcaryclark@google.com * 49e49fb63d355446b91d20ff78ad78b297e89a50dcaryclark@google.com * Use of this source code is governed by a BSD-style license that can be 59e49fb63d355446b91d20ff78ad78b297e89a50dcaryclark@google.com * found in the LICENSE file. 69e49fb63d355446b91d20ff78ad78b297e89a50dcaryclark@google.com */ 7639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com#include "DataTypes.h" 8639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com#include "Extrema.h" 9639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 10fa0588ff672564af1c235a63589573829035a60bcaryclark@google.comstatic int validUnitDivide(double numer, double denom, double* ratio) 11639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com{ 12fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com if (numer < 0) { 13639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com numer = -numer; 14639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com denom = -denom; 15639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com } 16639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com if (denom == 0 || numer == 0 || numer >= denom) 17639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com return 0; 18639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com double r = numer / denom; 19fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com if (r == 0) { // catch underflow if numer <<<< denom 20639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com return 0; 21fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com } 22639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com *ratio = r; 23639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com return 1; 24639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com} 25639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 26639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com/** From Numerical Recipes in C. 27639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 28639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com Q = -1/2 (B + sign(B) sqrt[B*B - 4*A*C]) 29639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com x1 = Q / A 30639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com x2 = C / Q 31639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com*/ 32fa0588ff672564af1c235a63589573829035a60bcaryclark@google.comstatic int findUnitQuadRoots(double A, double B, double C, double roots[2]) 33639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com{ 34639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com if (A == 0) 35fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com return validUnitDivide(-C, B, roots); 36639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 37639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com double* r = roots; 38639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 39639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com double R = B*B - 4*A*C; 40639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com if (R < 0) { // complex roots 41639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com return 0; 42639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com } 43639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com R = sqrt(R); 44639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 45639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com double Q = (B < 0) ? -(B-R)/2 : -(B+R)/2; 46fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com r += validUnitDivide(Q, A, r); 47fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com r += validUnitDivide(C, Q, r); 486d0032a8ec680221c2a704cac2391f2a2d77546fcaryclark@google.com if (r - roots == 2 && AlmostEqualUlps(roots[0], roots[1])) { // nearly-equal? 49639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com r -= 1; // skip the double root 50639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com } 51639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com return (int)(r - roots); 52639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com} 53639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 54639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com/** Cubic'(t) = At^2 + Bt + C, where 55639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com A = 3(-a + 3(b - c) + d) 56639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com B = 6(a - 2b + c) 57639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com C = 3(b - a) 58fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com Solve for t, keeping only those that fit between 0 < t < 1 59639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com*/ 60fa0588ff672564af1c235a63589573829035a60bcaryclark@google.comint findExtrema(double a, double b, double c, double d, double tValues[2]) 61639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com{ 62639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com // we divide A,B,C by 3 to simplify 63639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com double A = d - a + 3*(b - c); 64639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com double B = 2*(a - b - b + c); 65639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com double C = b - a; 66639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 67fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com return findUnitQuadRoots(A, B, C, tValues); 68639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com} 69639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com 70639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com/** Quad'(t) = At + B, where 71639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com A = 2(a - 2b + c) 72639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com B = 2(b - a) 73639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com Solve for t, only if it fits between 0 < t < 1 74639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com*/ 75fa0588ff672564af1c235a63589573829035a60bcaryclark@google.comint findExtrema(double a, double b, double c, double tValue[1]) 76639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com{ 77639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com /* At + B == 0 78639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com t = -B / A 79639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com */ 80fa0588ff672564af1c235a63589573829035a60bcaryclark@google.com return validUnitDivide(a - b, a - b - b + c, tValue); 81639df891487e40925a4f8d9a34fd3dc0c18b40a7caryclark@google.com} 82