v15i027: Graphics Gems, Part 5/5
Craig Kolb
craig at weedeater.math.yale.edu
Mon Oct 15 11:13:39 AEST 1990
Posting-number: Volume 15, Issue 27
Submitted-by: Craig Kolb <craig at weedeater.math.yale.edu>
Archive-name: ggems/part05
#! /bin/sh
# This is a shell archive. Remove anything before this line, then unpack
# it by saving it into a file and typing "sh file". To overwrite existing
# files, type "sh file -c". You can also feed this as standard input via
# unshar, or by typing "sh <file", e.g.. If this archive is complete, you
# will see the following message at the end:
# "End of archive 5 (of 5)."
# Contents: AALines/FastMatMul.c FitCurves.c GGVecLib.c NearestPoint.c
# Wrapped by craig at weedeater on Fri Oct 12 15:53:15 1990
PATH=/bin:/usr/bin:/usr/ucb ; export PATH
if test -f 'AALines/FastMatMul.c' -a "${1}" != "-c" ; then
echo shar: Will not clobber existing file \"'AALines/FastMatMul.c'\"
else
echo shar: Extracting \"'AALines/FastMatMul.c'\" \(12335 characters\)
sed "s/^X//" >'AALines/FastMatMul.c' <<'END_OF_FILE'
X/* FILENAME: FastMatMul.c [revised 18 AUG 90]
X
X AUTHOR: Kelvin Thompson
X
X DESCRIPTION: Routines to multiply different kinds of 4x4
X matrices as fast as possible. Based on ideas on pages 454,
X 460-461, and 646 of _Graphics_Gems_.
X
X These routines offer two advantages over the standard
X V3MatMul in the _Graphics_Gems_ vector library GGVecLib.c.
X First, the routines are faster. Second, they can handle
X input matrices that are the same as the output matrix.
X The routines have the disadvantage of taking more code
X space (from unwound loops).
X
X The routines are about as fast as you can get for
X general-purpose multiplication. If you have special
X knowledge about your system, you may be able to improve
X them a little more:
X
X [1] If you know that your input and output matrices will
X never overlap: remove the tests near the beginning and end of
X each routine, and just #define 'mptr' to 'result'. (The
X standard library's V3MatMul makes this assumption.)
X
X [2] If you know that your compiler supports more than
X three pointer-to-double registers in a subroutine: make
X 'result' in each routine a register variable. You might
X also make the 'usetemp' boolean a register.
X
X [3] If you have limited stack space, or your system can
X access global memory faster than stack: make each routine's
X 'tempx' a static, or let all routines share a global 'tempx'.
X (This is useless if assumption [1] holds.)
X*/
X
X/* definitions from "GraphicsGems.h" */
Xtypedef struct Matrix3Struct { /* 3-by-3 matrix */
X double element[3][3];
X } Matrix3;
Xtypedef struct Matrix4Struct { /* 4-by-4 matrix */
X double element[4][4];
X } Matrix4;
X
X/* routines in this file */
XMatrix3 *V2MatMul(); /* general 3x3 matrix multiplier */
XMatrix4 *V3MatMul(); /* general 4x4 matrix multiplier */
XMatrix4 *V3AffMatMul(); /* affine 4x4 matrix multiplier */
XMatrix4 *V3LinMatMul(); /* linear 4x4 matrix multiplier */
X
X/* macro to access matrix element */
X#define MVAL(mptr,row,col) ((mptr)->element[row][col])
X
X
X
X
X
X/* ************************************
X * *
X * V2MatMul *
X * *
X ************************************
X
X DESCRIPTION: Multiply two general 3x3 matricies. If one of
X the input matrices is the same as the output, write the
X result to a temporary matrix during multiplication, then
X copy to the output matrix.
X
X ENTRY:
X a -- pointer to left matrix
X b -- pointer to right matrix
X result -- result matrix
X
X EXIT: returns 'result'
X*/
X
XMatrix3 *V2MatMul ( a, b, result )
Xregister Matrix3 *a,*b;
XMatrix3 *result;
X{
Xregister Matrix3 *mptr;
Xint usetemp; /* boolean */
XMatrix3 tempx;
X
X/* decide where intermediate result goes */
Xusetemp = ( a == result || b == result );
Xif ( usetemp )
X mptr = & tempx;
Xelse
X mptr = result;
X
XMVAL(mptr,0,0) =
X MVAL(a,0,0) * MVAL(b,0,0)
X + MVAL(a,0,1) * MVAL(b,1,0)
X + MVAL(a,0,2) * MVAL(b,2,0);
X
XMVAL(mptr,0,1) =
X MVAL(a,0,0) * MVAL(b,0,1)
X + MVAL(a,0,1) * MVAL(b,1,1)
X + MVAL(a,0,2) * MVAL(b,2,1);
X
XMVAL(mptr,0,2) =
X MVAL(a,0,0) * MVAL(b,0,2)
X + MVAL(a,0,1) * MVAL(b,1,2)
X + MVAL(a,0,2) * MVAL(b,2,2);
X
XMVAL(mptr,1,0) =
X MVAL(a,1,0) * MVAL(b,0,0)
X + MVAL(a,1,1) * MVAL(b,1,0)
X + MVAL(a,1,2) * MVAL(b,2,0);
X
XMVAL(mptr,1,1) =
X MVAL(a,1,0) * MVAL(b,0,1)
X + MVAL(a,1,1) * MVAL(b,1,1)
X + MVAL(a,1,2) * MVAL(b,2,1);
X
XMVAL(mptr,1,2) =
X MVAL(a,1,0) * MVAL(b,0,2)
X + MVAL(a,1,1) * MVAL(b,1,2)
X + MVAL(a,1,2) * MVAL(b,2,2);
X
XMVAL(mptr,2,0) =
X MVAL(a,2,0) * MVAL(b,0,0)
X + MVAL(a,2,1) * MVAL(b,1,0)
X + MVAL(a,2,2) * MVAL(b,2,0);
X
XMVAL(mptr,2,1) =
X MVAL(a,2,0) * MVAL(b,0,1)
X + MVAL(a,2,1) * MVAL(b,1,1)
X + MVAL(a,2,2) * MVAL(b,2,1);
X
XMVAL(mptr,2,2) =
X MVAL(a,2,0) * MVAL(b,0,2)
X + MVAL(a,2,1) * MVAL(b,1,2)
X + MVAL(a,2,2) * MVAL(b,2,2);
X
X/* copy temp matrix to result if needed */
Xif ( usetemp )
X *result = *mptr;
X
Xreturn result;
X}
X
X
X
X
X/* ************************************
X * *
X * V3MatMul *
X * *
X ************************************
X
X DESCRIPTION: Multiply two general 4x4 matricies. If one of
X the input matrices is the same as the output, write the
X result to a temporary matrix during multiplication, then
X copy to the output matrix.
X
X ENTRY:
X a -- pointer to left matrix
X b -- pointer to right matrix
X result -- result matrix
X
X EXIT: returns 'result'
X*/
X
XMatrix4 *V3MatMul ( a, b, result )
Xregister Matrix4 *a,*b;
XMatrix4 *result;
X{
Xregister Matrix4 *mptr;
Xint usetemp; /* boolean */
XMatrix4 tempx;
X
X/* decide where intermediate result goes */
Xusetemp = ( a == result || b == result );
Xif ( usetemp )
X mptr = & tempx;
Xelse
X mptr = result;
X
XMVAL(mptr,0,0) =
X MVAL(a,0,0) * MVAL(b,0,0)
X + MVAL(a,0,1) * MVAL(b,1,0)
X + MVAL(a,0,2) * MVAL(b,2,0)
X + MVAL(a,0,3) * MVAL(b,3,0);
X
XMVAL(mptr,0,1) =
X MVAL(a,0,0) * MVAL(b,0,1)
X + MVAL(a,0,1) * MVAL(b,1,1)
X + MVAL(a,0,2) * MVAL(b,2,1)
X + MVAL(a,0,3) * MVAL(b,3,1);
X
XMVAL(mptr,0,2) =
X MVAL(a,0,0) * MVAL(b,0,2)
X + MVAL(a,0,1) * MVAL(b,1,2)
X + MVAL(a,0,2) * MVAL(b,2,2)
X + MVAL(a,0,3) * MVAL(b,3,2);
X
XMVAL(mptr,0,3) =
X MVAL(a,0,0) * MVAL(b,0,3)
X + MVAL(a,0,1) * MVAL(b,1,3)
X + MVAL(a,0,2) * MVAL(b,2,3)
X + MVAL(a,0,3) * MVAL(b,3,3);
X
XMVAL(mptr,1,0) =
X MVAL(a,1,0) * MVAL(b,0,0)
X + MVAL(a,1,1) * MVAL(b,1,0)
X + MVAL(a,1,2) * MVAL(b,2,0)
X + MVAL(a,1,3) * MVAL(b,3,0);
X
XMVAL(mptr,1,1) =
X MVAL(a,1,0) * MVAL(b,0,1)
X + MVAL(a,1,1) * MVAL(b,1,1)
X + MVAL(a,1,2) * MVAL(b,2,1)
X + MVAL(a,1,3) * MVAL(b,3,1);
X
XMVAL(mptr,1,2) =
X MVAL(a,1,0) * MVAL(b,0,2)
X + MVAL(a,1,1) * MVAL(b,1,2)
X + MVAL(a,1,2) * MVAL(b,2,2)
X + MVAL(a,1,3) * MVAL(b,3,2);
X
XMVAL(mptr,1,3) =
X MVAL(a,1,0) * MVAL(b,0,3)
X + MVAL(a,1,1) * MVAL(b,1,3)
X + MVAL(a,1,2) * MVAL(b,2,3)
X + MVAL(a,1,3) * MVAL(b,3,3);
X
XMVAL(mptr,2,0) =
X MVAL(a,2,0) * MVAL(b,0,0)
X + MVAL(a,2,1) * MVAL(b,1,0)
X + MVAL(a,2,2) * MVAL(b,2,0)
X + MVAL(a,2,3) * MVAL(b,3,0);
X
XMVAL(mptr,2,1) =
X MVAL(a,2,0) * MVAL(b,0,1)
X + MVAL(a,2,1) * MVAL(b,1,1)
X + MVAL(a,2,2) * MVAL(b,2,1)
X + MVAL(a,2,3) * MVAL(b,3,1);
X
XMVAL(mptr,2,2) =
X MVAL(a,2,0) * MVAL(b,0,2)
X + MVAL(a,2,1) * MVAL(b,1,2)
X + MVAL(a,2,2) * MVAL(b,2,2)
X + MVAL(a,2,3) * MVAL(b,3,2);
X
XMVAL(mptr,2,3) =
X MVAL(a,2,0) * MVAL(b,0,3)
X + MVAL(a,2,1) * MVAL(b,1,3)
X + MVAL(a,2,2) * MVAL(b,2,3)
X + MVAL(a,2,3) * MVAL(b,3,3);
X
XMVAL(mptr,3,0) =
X MVAL(a,3,0) * MVAL(b,0,0)
X + MVAL(a,3,1) * MVAL(b,1,0)
X + MVAL(a,3,2) * MVAL(b,2,0)
X + MVAL(a,3,3) * MVAL(b,3,0);
X
XMVAL(mptr,3,1) =
X MVAL(a,3,0) * MVAL(b,0,1)
X + MVAL(a,3,1) * MVAL(b,1,1)
X + MVAL(a,3,2) * MVAL(b,2,1)
X + MVAL(a,3,3) * MVAL(b,3,1);
X
XMVAL(mptr,3,2) =
X MVAL(a,3,0) * MVAL(b,0,2)
X + MVAL(a,3,1) * MVAL(b,1,2)
X + MVAL(a,3,2) * MVAL(b,2,2)
X + MVAL(a,3,3) * MVAL(b,3,2);
X
XMVAL(mptr,3,3) =
X MVAL(a,3,0) * MVAL(b,0,3)
X + MVAL(a,3,1) * MVAL(b,1,3)
X + MVAL(a,3,2) * MVAL(b,2,3)
X + MVAL(a,3,3) * MVAL(b,3,3);
X
X/* copy temp matrix to result if needed */
Xif ( usetemp )
X *result = *mptr;
X
Xreturn result;
X}
X
X
X
X
X
X/* ************************************
X * *
X * V3AffMatMul *
X * *
X ************************************
X
X DESCRIPTION: Multiply two affine 4x4 matricies. The
X routine assumes the rightmost column of each input
X matrix is [0 0 0 1]. The output matrix will have the
X same property.
X
X If one of the input matrices is the same as the output,
X write the result to a temporary matrix during multiplication,
X then copy to the output matrix.
X
X ENTRY:
X a -- pointer to left matrix
X b -- pointer to right matrix
X result -- result matrix
X
X EXIT: returns 'result'
X*/
X
XMatrix4 *V3AffMatMul ( a, b, result )
Xregister Matrix4 *a,*b;
XMatrix4 *result;
X{
Xregister Matrix4 *mptr;
Xint usetemp; /* boolean */
XMatrix4 tempx;
X
X/* decide where intermediate result goes */
Xusetemp = ( a == result || b == result );
Xif ( usetemp )
X mptr = & tempx;
Xelse
X mptr = result;
X
XMVAL(mptr,0,0) =
X MVAL(a,0,0) * MVAL(b,0,0)
X + MVAL(a,0,1) * MVAL(b,1,0)
X + MVAL(a,0,2) * MVAL(b,2,0);
X
XMVAL(mptr,0,1) =
X MVAL(a,0,0) * MVAL(b,0,1)
X + MVAL(a,0,1) * MVAL(b,1,1)
X + MVAL(a,0,2) * MVAL(b,2,1);
X
XMVAL(mptr,0,2) =
X MVAL(a,0,0) * MVAL(b,0,2)
X + MVAL(a,0,1) * MVAL(b,1,2)
X + MVAL(a,0,2) * MVAL(b,2,2);
X
XMVAL(mptr,0,3) = 0.0;
X
XMVAL(mptr,1,0) =
X MVAL(a,1,0) * MVAL(b,0,0)
X + MVAL(a,1,1) * MVAL(b,1,0)
X + MVAL(a,1,2) * MVAL(b,2,0);
X
XMVAL(mptr,1,1) =
X MVAL(a,1,0) * MVAL(b,0,1)
X + MVAL(a,1,1) * MVAL(b,1,1)
X + MVAL(a,1,2) * MVAL(b,2,1);
X
XMVAL(mptr,1,2) =
X MVAL(a,1,0) * MVAL(b,0,2)
X + MVAL(a,1,1) * MVAL(b,1,2)
X + MVAL(a,1,2) * MVAL(b,2,2);
X
XMVAL(mptr,1,3) = 0.0;
X
XMVAL(mptr,2,0) =
X MVAL(a,2,0) * MVAL(b,0,0)
X + MVAL(a,2,1) * MVAL(b,1,0)
X + MVAL(a,2,2) * MVAL(b,2,0);
X
XMVAL(mptr,2,1) =
X MVAL(a,2,0) * MVAL(b,0,1)
X + MVAL(a,2,1) * MVAL(b,1,1)
X + MVAL(a,2,2) * MVAL(b,2,1);
X
XMVAL(mptr,2,2) =
X MVAL(a,2,0) * MVAL(b,0,2)
X + MVAL(a,2,1) * MVAL(b,1,2)
X + MVAL(a,2,2) * MVAL(b,2,2);
X
XMVAL(mptr,2,3) = 0.0;
X
XMVAL(mptr,3,0) =
X MVAL(a,3,0) * MVAL(b,0,0)
X + MVAL(a,3,1) * MVAL(b,1,0)
X + MVAL(a,3,2) * MVAL(b,2,0)
X + MVAL(b,3,0);
X
XMVAL(mptr,3,1) =
X MVAL(a,3,0) * MVAL(b,0,1)
X + MVAL(a,3,1) * MVAL(b,1,1)
X + MVAL(a,3,2) * MVAL(b,2,1)
X + MVAL(b,3,1);
X
XMVAL(mptr,3,2) =
X MVAL(a,3,0) * MVAL(b,0,2)
X + MVAL(a,3,1) * MVAL(b,1,2)
X + MVAL(a,3,2) * MVAL(b,2,2)
X + MVAL(b,3,2);
X
XMVAL(mptr,3,3) = 1.0;
X
X/* copy temp matrix to result if needed */
Xif ( usetemp )
X *result = *mptr;
X
Xreturn result;
X}
X
X
X
X
X
X/* ************************************
X * *
X * V3LinMatMul *
X * *
X ************************************
X
X DESCRIPTION: Multiply two affine 4x4 matricies. The
X routine assumes the right column and bottom line
X of each input matrix is [0 0 0 1]. The output matrix
X will have the same property. This is pretty much the
X same thing as multiplying two 3x3 matrices.
X
X If one of the input matrices is the same as the output,
X write the result to a temporary matrix during multiplication,
X then copy to the output matrix.
X
X ENTRY:
X a -- pointer to left matrix
X b -- pointer to right matrix
X result -- result matrix
X
X EXIT: returns 'result'
X*/
X
XMatrix4 *V3LinMatMul ( a, b, result )
Xregister Matrix4 *a,*b;
XMatrix4 *result;
X{
Xregister Matrix4 *mptr;
Xint usetemp; /* boolean */
XMatrix4 tempx;
X
X/* decide where intermediate result goes */
Xusetemp = ( a == result || b == result );
Xif ( usetemp )
X mptr = & tempx;
Xelse
X mptr = result;
X
XMVAL(mptr,0,0) =
X MVAL(a,0,0) * MVAL(b,0,0)
X + MVAL(a,0,1) * MVAL(b,1,0)
X + MVAL(a,0,2) * MVAL(b,2,0);
X
XMVAL(mptr,0,1) =
X MVAL(a,0,0) * MVAL(b,0,1)
X + MVAL(a,0,1) * MVAL(b,1,1)
X + MVAL(a,0,2) * MVAL(b,2,1);
X
XMVAL(mptr,0,2) =
X MVAL(a,0,0) * MVAL(b,0,2)
X + MVAL(a,0,1) * MVAL(b,1,2)
X + MVAL(a,0,2) * MVAL(b,2,2);
X
XMVAL(mptr,0,3) = 0.0;
X
XMVAL(mptr,1,0) =
X MVAL(a,1,0) * MVAL(b,0,0)
X + MVAL(a,1,1) * MVAL(b,1,0)
X + MVAL(a,1,2) * MVAL(b,2,0);
X
XMVAL(mptr,1,1) =
X MVAL(a,1,0) * MVAL(b,0,1)
X + MVAL(a,1,1) * MVAL(b,1,1)
X + MVAL(a,1,2) * MVAL(b,2,1);
X
XMVAL(mptr,1,2) =
X MVAL(a,1,0) * MVAL(b,0,2)
X + MVAL(a,1,1) * MVAL(b,1,2)
X + MVAL(a,1,2) * MVAL(b,2,2);
X
XMVAL(mptr,1,3) = 0.0;
X
XMVAL(mptr,2,0) =
X MVAL(a,2,0) * MVAL(b,0,0)
X + MVAL(a,2,1) * MVAL(b,1,0)
X + MVAL(a,2,2) * MVAL(b,2,0);
X
XMVAL(mptr,2,1) =
X MVAL(a,2,0) * MVAL(b,0,1)
X + MVAL(a,2,1) * MVAL(b,1,1)
X + MVAL(a,2,2) * MVAL(b,2,1);
X
XMVAL(mptr,2,2) =
X MVAL(a,2,0) * MVAL(b,0,2)
X + MVAL(a,2,1) * MVAL(b,1,2)
X + MVAL(a,2,2) * MVAL(b,2,2);
X
XMVAL(mptr,2,3) = 0.0;
X
XMVAL(mptr,3,0) = 0.0;
XMVAL(mptr,3,1) = 0.0;
XMVAL(mptr,3,2) = 0.0;
XMVAL(mptr,3,3) = 1.0;
X
X/* copy temp matrix to result if needed */
Xif ( usetemp )
X *result = *mptr;
X
Xreturn result;
X}
END_OF_FILE
if test 12335 -ne `wc -c <'AALines/FastMatMul.c'`; then
echo shar: \"'AALines/FastMatMul.c'\" unpacked with wrong size!
fi
# end of 'AALines/FastMatMul.c'
fi
if test -f 'FitCurves.c' -a "${1}" != "-c" ; then
echo shar: Will not clobber existing file \"'FitCurves.c'\"
else
echo shar: Extracting \"'FitCurves.c'\" \(14420 characters\)
sed "s/^X//" >'FitCurves.c' <<'END_OF_FILE'
X/*
XAn Algorithm for Automatically Fitting Digitized Curves
Xby Philip J. Schneider
Xfrom "Graphics Gems", Academic Press, 1990
X*/
X
X#define TESTMODE
X
X/* fit_cubic.c */
X/* Piecewise cubic fitting code */
X
X#include "GraphicsGems.h"
X#include <stdio.h>
X#include <malloc.h>
X#include <math.h>
X
Xtypedef Point2 *BezierCurve;
X
X/* Forward declarations */
Xvoid FitCurve();
Xstatic void FitCubic();
Xstatic double *Reparameterize();
Xstatic double NewtonRaphsonRootFind();
Xstatic Point2 Bezier();
Xstatic double B0(), B1(), B2(), B3();
Xstatic Vector2 ComputeLeftTangent();
Xstatic Vector2 ComputeRightTangent();
Xstatic Vector2 ComputeCenterTangent();
Xstatic double ComputeMaxError();
Xstatic double *ChordLengthParameterize();
Xstatic BezierCurve GenerateBezier();
Xstatic Vector2 V2AddII();
Xstatic Vector2 V2ScaleII();
Xstatic Vector2 V2SubII();
X
X#define MAXPOINTS 1000 /* The most points you can have */
X
X#ifdef TESTMODE
X
XDrawBezierCurve(n, curve)
Xint n;
XBezierCurve curve;
X{
X /* You'll have to write this yourself. */
X}
X
X/*
X * main:
X * Example of how to use the curve-fitting code. Given an array
X * of points and a tolerance (squared error between points and
X * fitted curve), the algorithm will generate a piecewise
X * cubic Bezier representation that approximates the points.
X * When a cubic is generated, the routine "DrawBezierCurve"
X * is called, which outputs the Bezier curve just created
X * (arguments are the degree and the control points, respectively).
X * Users will have to implement this function themselves
X * ascii output, etc.
X *
X */
Xmain()
X{
X static Point2 d[7] = { /* Digitized points */
X { 0.0, 0.0 },
X { 0.0, 0.5 },
X { 1.1, 1.4 },
X { 2.1, 1.6 },
X { 3.2, 1.1 },
X { 4.0, 0.2 },
X { 4.0, 0.0 },
X };
X double error = 4.0; /* Squared error */
X FitCurve(d, 7, error); /* Fit the Bezier curves */
X}
X#endif /* TESTMODE */
X
X/*
X * FitCurve :
X * Fit a Bezier curve to a set of digitized points
X */
Xvoid FitCurve(d, nPts, error)
X Point2 *d; /* Array of digitized points */
X int nPts; /* Number of digitized points */
X double error; /* User-defined error squared */
X{
X Vector2 tHat1, tHat2; /* Unit tangent vectors at endpoints */
X
X tHat1 = ComputeLeftTangent(d, 0);
X tHat2 = ComputeRightTangent(d, nPts - 1);
X FitCubic(d, 0, nPts - 1, tHat1, tHat2, error);
X}
X
X
X
X/*
X * FitCubic :
X * Fit a Bezier curve to a (sub)set of digitized points
X */
Xstatic void FitCubic(d, first, last, tHat1, tHat2, error)
X Point2 *d; /* Array of digitized points */
X int first, last; /* Indices of first and last pts in region */
X Vector2 tHat1, tHat2; /* Unit tangent vectors at endpoints */
X double error; /* User-defined error squared */
X{
X BezierCurve bezCurve; /*Control points of fitted Bezier curve*/
X double *u; /* Parameter values for point */
X double *uPrime; /* Improved parameter values */
X double maxError; /* Maximum fitting error */
X int splitPoint; /* Point to split point set at */
X int nPts; /* Number of points in subset */
X double iterationError; /*Error below which you try iterating */
X int maxIterations = 4; /* Max times to try iterating */
X Vector2 tHatCenter; /* Unit tangent vector at splitPoint */
X int i;
X
X iterationError = error * error;
X nPts = last - first + 1;
X
X /* Use heuristic if region only has two points in it */
X if (nPts == 2) {
X double dist = V2DistanceBetween2Points(&d[last], &d[first]) / 3.0;
X
X bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
X bezCurve[0] = d[first];
X bezCurve[3] = d[last];
X V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
X V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
X DrawBezierCurve(3, bezCurve);
X return;
X }
X
X /* Parameterize points, and attempt to fit curve */
X u = ChordLengthParameterize(d, first, last);
X bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
X
X /* Find max deviation of points to fitted curve */
X maxError = ComputeMaxError(d, first, last, bezCurve, u, &splitPoint);
X if (maxError < error) {
X DrawBezierCurve(3, bezCurve);
X return;
X }
X
X
X /* If error not too large, try some reparameterization */
X /* and iteration */
X if (maxError < iterationError) {
X for (i = 0; i < maxIterations; i++) {
X uPrime = Reparameterize(d, first, last, u, bezCurve);
X bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
X maxError = ComputeMaxError(d, first, last,
X bezCurve, uPrime, &splitPoint);
X if (maxError < error) {
X DrawBezierCurve(3, bezCurve);
X return;
X }
X free((char *)u);
X u = uPrime;
X }
X}
X
X /* Fitting failed -- split at max error point and fit recursively */
X tHatCenter = ComputeCenterTangent(d, splitPoint);
X FitCubic(d, first, splitPoint, tHat1, tHatCenter, error);
X V2Negate(&tHatCenter);
X FitCubic(d, splitPoint, last, tHatCenter, tHat2, error);
X}
X
X
X/*
X * GenerateBezier :
X * Use least-squares method to find Bezier control points for region.
X *
X */
Xstatic BezierCurve GenerateBezier(d, first, last, uPrime, tHat1, tHat2)
X Point2 *d; /* Array of digitized points */
X int first, last; /* Indices defining region */
X double *uPrime; /* Parameter values for region */
X Vector2 tHat1, tHat2; /* Unit tangents at endpoints */
X{
X int i;
X Vector2 A[MAXPOINTS][2]; /* Precomputed rhs for eqn */
X int nPts; /* Number of pts in sub-curve */
X double C[2][2]; /* Matrix C */
X double X[2]; /* Matrix X */
X double det_C0_C1, /* Determinants of matrices */
X det_C0_X,
X det_X_C1;
X double alpha_l, /* Alpha values, left and right */
X alpha_r;
X Vector2 tmp; /* Utility variable */
X BezierCurve bezCurve; /* RETURN bezier curve ctl pts */
X
X bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
X nPts = last - first + 1;
X
X
X /* Compute the A's */
X for (i = 0; i < nPts; i++) {
X Vector2 v1, v2;
X v1 = tHat1;
X v2 = tHat2;
X V2Scale(&v1, B1(uPrime[i]));
X V2Scale(&v2, B2(uPrime[i]));
X A[i][0] = v1;
X A[i][1] = v2;
X }
X
X /* Create the C and X matrices */
X C[0][0] = 0.0;
X C[0][1] = 0.0;
X C[1][0] = 0.0;
X C[1][1] = 0.0;
X X[0] = 0.0;
X X[1] = 0.0;
X
X for (i = 0; i < nPts; i++) {
X C[0][0] += V2Dot(&A[i][0], &A[i][0]);
X C[0][1] += V2Dot(&A[i][0], &A[i][1]);
X/* C[1][0] += V2Dot(&A[i][0], &A[i][1]);*/
X C[1][0] = C[0][1];
X C[1][1] += V2Dot(&A[i][1], &A[i][1]);
X
X tmp = V2SubII(d[first + i],
X V2AddII(
X V2ScaleII(d[first], B0(uPrime[i])),
X V2AddII(
X V2ScaleII(d[first], B1(uPrime[i])),
X V2AddII(
X V2ScaleII(d[last], B2(uPrime[i])),
X V2ScaleII(d[last], B3(uPrime[i]))))));
X
X
X X[0] += V2Dot(&A[i][0], &tmp);
X X[1] += V2Dot(&A[i][1], &tmp);
X }
X
X /* Compute the determinants of C and X */
X det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
X det_C0_X = C[0][0] * X[1] - C[0][1] * X[0];
X det_X_C1 = X[0] * C[1][1] - X[1] * C[0][1];
X
X /* Finally, derive alpha values */
X if (det_C0_C1 == 0.0) {
X det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
X }
X alpha_l = det_X_C1 / det_C0_C1;
X alpha_r = det_C0_X / det_C0_C1;
X
X
X /* If alpha negative, use the Wu/Barsky heuristic (see text) */
X if (alpha_l < 0.0 || alpha_r < 0.0) {
X double dist = V2DistanceBetween2Points(&d[last], &d[first]) /
X 3.0;
X
X bezCurve[0] = d[first];
X bezCurve[3] = d[last];
X V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
X V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
X return (bezCurve);
X }
X
X /* First and last control points of the Bezier curve are */
X /* positioned exactly at the first and last data points */
X /* Control points 1 and 2 are positioned an alpha distance out */
X /* on the tangent vectors, left and right, respectively */
X bezCurve[0] = d[first];
X bezCurve[3] = d[last];
X V2Add(&bezCurve[0], V2Scale(&tHat1, alpha_l), &bezCurve[1]);
X V2Add(&bezCurve[3], V2Scale(&tHat2, alpha_r), &bezCurve[2]);
X return (bezCurve);
X}
X
X
X/*
X * Reparameterize:
X * Given set of points and their parameterization, try to find
X * a better parameterization.
X *
X */
Xstatic double *Reparameterize(d, first, last, u, bezCurve)
X Point2 *d; /* Array of digitized points */
X int first, last; /* Indices defining region */
X double *u; /* Current parameter values */
X BezierCurve bezCurve; /* Current fitted curve */
X{
X int nPts = last-first+1;
X int i;
X double *uPrime; /* New parameter values */
X
X uPrime = (double *)malloc(nPts * sizeof(double));
X for (i = first; i <= last; i++) {
X uPrime[i-first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i-
X first]);
X }
X return (uPrime);
X}
X
X
X
X/*
X * NewtonRaphsonRootFind :
X * Use Newton-Raphson iteration to find better root.
X */
Xstatic double NewtonRaphsonRootFind(Q, P, u)
X BezierCurve Q; /* Current fitted curve */
X Point2 P; /* Digitized point */
X double u; /* Parameter value for "P" */
X{
X double numerator, denominator;
X Point2 Q1[3], Q2[2]; /* Q' and Q'' */
X Point2 Q_u, Q1_u, Q2_u; /*u evaluated at Q, Q', & Q'' */
X double uPrime; /* Improved u */
X int i;
X
X /* Compute Q(u) */
X Q_u = Bezier(3, Q, u);
X
X /* Generate control vertices for Q' */
X for (i = 0; i <= 2; i++) {
X Q1[i].x = (Q[i+1].x - Q[i].x) * 3.0;
X Q1[i].y = (Q[i+1].y - Q[i].y) * 3.0;
X }
X
X /* Generate control vertices for Q'' */
X for (i = 0; i <= 1; i++) {
X Q2[i].x = (Q1[i+1].x - Q1[i].x) * 2.0;
X Q2[i].y = (Q1[i+1].y - Q1[i].y) * 2.0;
X }
X
X /* Compute Q'(u) and Q''(u) */
X Q1_u = Bezier(2, Q1, u);
X Q2_u = Bezier(1, Q2, u);
X
X /* Compute f(u)/f'(u) */
X numerator = (Q_u.x - P.x) * (Q1_u.x) + (Q_u.y - P.y) * (Q1_u.y);
X denominator = (Q1_u.x) * (Q1_u.x) + (Q1_u.y) * (Q1_u.y) +
X (Q_u.x - P.x) * (Q2_u.x) + (Q_u.y - P.y) * (Q2_u.y);
X
X /* u = u - f(u)/f'(u) */
X uPrime = u - (numerator/denominator);
X return (uPrime);
X}
X
X
X
X/*
X * Bezier :
X * Evaluate a Bezier curve at a particular parameter value
X *
X */
Xstatic Point2 Bezier(degree, V, t)
X int degree; /* The degree of the bezier curve */
X Point2 *V; /* Array of control points */
X double t; /* Parametric value to find point for */
X{
X int i, j;
X Point2 Q; /* Point on curve at parameter t */
X Point2 *Vtemp; /* Local copy of control points */
X
X /* Copy array */
X Vtemp = (Point2 *)malloc((unsigned)((degree+1)
X * sizeof (Point2)));
X for (i = 0; i <= degree; i++) {
X Vtemp[i] = V[i];
X }
X
X /* Triangle computation */
X for (i = 1; i <= degree; i++) {
X for (j = 0; j <= degree-i; j++) {
X Vtemp[j].x = (1.0 - t) * Vtemp[j].x + t * Vtemp[j+1].x;
X Vtemp[j].y = (1.0 - t) * Vtemp[j].y + t * Vtemp[j+1].y;
X }
X }
X
X Q = Vtemp[0];
X free((char *)Vtemp);
X return Q;
X}
X
X
X/*
X * B0, B1, B2, B3 :
X * Bezier multipliers
X */
Xstatic double B0(u)
X double u;
X{
X double tmp = 1.0 - u;
X return (tmp * tmp * tmp);
X}
X
X
Xstatic double B1(u)
X double u;
X{
X double tmp = 1.0 - u;
X return (3 * u * (tmp * tmp));
X}
X
Xstatic double B2(u)
X double u;
X{
X double tmp = 1.0 - u;
X return (3 * u * u * tmp);
X}
X
Xstatic double B3(u)
X double u;
X{
X return (u * u * u);
X}
X
X
X
X/*
X * ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
X *Approximate unit tangents at endpoints and "center" of digitized curve
X */
Xstatic Vector2 ComputeLeftTangent(d, end)
X Point2 *d; /* Digitized points*/
X int end; /* Index to "left" end of region */
X{
X Vector2 tHat1;
X tHat1 = V2SubII(d[end+1], d[end]);
X tHat1 = *V2Normalize(&tHat1);
X return tHat1;
X}
X
Xstatic Vector2 ComputeRightTangent(d, end)
X Point2 *d; /* Digitized points */
X int end; /* Index to "right" end of region */
X{
X Vector2 tHat2;
X tHat2 = V2SubII(d[end-1], d[end]);
X tHat2 = *V2Normalize(&tHat2);
X return tHat2;
X}
X
X
Xstatic Vector2 ComputeCenterTangent(d, center)
X Point2 *d; /* Digitized points */
X int center; /* Index to point inside region */
X{
X Vector2 V1, V2, tHatCenter;
X
X V1 = V2SubII(d[center-1], d[center]);
X V2 = V2SubII(d[center], d[center+1]);
X tHatCenter.x = (V1.x + V2.x)/2.0;
X tHatCenter.y = (V1.y + V2.y)/2.0;
X tHatCenter = *V2Normalize(&tHatCenter);
X return tHatCenter;
X}
X
X
X/*
X * ChordLengthParameterize :
X * Assign parameter values to digitized points
X * using relative distances between points.
X */
Xstatic double *ChordLengthParameterize(d, first, last)
X Point2 *d; /* Array of digitized points */
X int first, last; /* Indices defining region */
X{
X int i;
X double *u; /* Parameterization */
X
X u = (double *)malloc((unsigned)(last-first+1) * sizeof(double));
X
X u[0] = 0.0;
X for (i = first+1; i <= last; i++) {
X u[i-first] = u[i-first-1] +
X V2DistanceBetween2Points(&d[i], &d[i-1]);
X }
X
X for (i = first + 1; i <= last; i++) {
X u[i-first] = u[i-first] / u[last-first];
X }
X
X return(u);
X}
X
X
X
X
X/*
X * ComputeMaxError :
X * Find the maximum squared distance of digitized points
X * to fitted curve.
X*/
Xstatic double ComputeMaxError(d, first, last, bezCurve, u, splitPoint)
X Point2 *d; /* Array of digitized points */
X int first, last; /* Indices defining region */
X BezierCurve bezCurve; /* Fitted Bezier curve */
X double *u; /* Parameterization of points */
X int *splitPoint; /* Point of maximum error */
X{
X int i;
X double maxDist; /* Maximum error */
X double dist; /* Current error */
X Point2 P; /* Point on curve */
X Vector2 v; /* Vector from point to curve */
X
X *splitPoint = (last - first + 1)/2;
X maxDist = 0.0;
X for (i = first + 1; i < last; i++) {
X P = Bezier(3, bezCurve, u[i-first]);
X v = V2SubII(P, d[i]);
X dist = V2SquaredLength(&v);
X if (dist >= maxDist) {
X maxDist = dist;
X *splitPoint = i;
X }
X }
X return (maxDist);
X}
Xstatic Vector2 V2AddII(a, b)
X Vector2 a, b;
X{
X Vector2 c;
X c.x = a.x + b.x; c.y = a.y + b.y;
X return (c);
X}
Xstatic Vector2 V2ScaleII(v, s)
X Vector2 v;
X double s;
X{
X Vector2 result;
X result.x = v.x * s; result.y = v.y * s;
X return (result);
X}
X
Xstatic Vector2 V2SubII(a, b)
X Vector2 a, b;
X{
X Vector2 c;
X c.x = a.x - b.x; c.y = a.y - b.y;
X return (c);
X}
END_OF_FILE
if test 14420 -ne `wc -c <'FitCurves.c'`; then
echo shar: \"'FitCurves.c'\" unpacked with wrong size!
fi
# end of 'FitCurves.c'
fi
if test -f 'GGVecLib.c' -a "${1}" != "-c" ; then
echo shar: Will not clobber existing file \"'GGVecLib.c'\"
else
echo shar: Extracting \"'GGVecLib.c'\" \(10092 characters\)
sed "s/^X//" >'GGVecLib.c' <<'END_OF_FILE'
X/*
X2d and 3d Vector C Library
Xby Andrew Glassner
Xfrom "Graphics Gems", Academic Press, 1990
X*/
X
X#include <math.h>
X#include "GraphicsGems.h"
X
X/******************/
X/* 2d Library */
X/******************/
X
X/* returns squared length of input vector */
Xdouble V2SquaredLength(a)
XVector2 *a;
X{ return((a->x * a->x)+(a->y * a->y));
X };
X
X/* returns length of input vector */
Xdouble V2Length(a)
XVector2 *a;
X{
X return(sqrt(V2SquaredLength(a)));
X };
X
X/* negates the input vector and returns it */
XVector2 *V2Negate(v)
XVector2 *v;
X{
X v->x = -v->x; v->y = -v->y;
X return(v);
X };
X
X/* normalizes the input vector and returns it */
XVector2 *V2Normalize(v)
XVector2 *v;
X{
Xdouble len = V2Length(v);
X if (len != 0.0) { v->x /= len; v->y /= len; };
X return(v);
X };
X
X
X/* scales the input vector to the new length and returns it */
XVector2 *V2Scale(v, newlen)
XVector2 *v;
Xdouble newlen;
X{
Xdouble len = V2Length(v);
X if (len != 0.0) { v->x *= newlen/len; v->y *= newlen/len; };
X return(v);
X };
X
X/* return vector sum c = a+b */
XVector2 *V2Add(a, b, c)
XVector2 *a, *b, *c;
X{
X c->x = a->x+b->x; c->y = a->y+b->y;
X return(c);
X };
X
X/* return vector difference c = a-b */
XVector2 *V2Sub(a, b, c)
XVector2 *a, *b, *c;
X{
X c->x = a->x-b->x; c->y = a->y-b->y;
X return(c);
X };
X
X/* return the dot product of vectors a and b */
Xdouble V2Dot(a, b)
XVector2 *a, *b;
X{
X return((a->x*b->x)+(a->y*b->y));
X };
X
X/* linearly interpolate between vectors by an amount alpha */
X/* and return the resulting vector. */
X/* When alpha=0, result=lo. When alpha=1, result=hi. */
XVector2 *V2Lerp(lo, hi, alpha, result)
XVector2 *lo, *hi, *result;
Xdouble alpha;
X{
X result->x = LERP(alpha, lo->x, hi->x);
X result->y = LERP(alpha, lo->y, hi->y);
X return(result);
X };
X
X
X/* make a linear combination of two vectors and return the result. */
X/* result = (a * ascl) + (b * bscl) */
XVector2 *V2Combine (a, b, result, ascl, bscl)
XVector2 *a, *b, *result;
Xdouble ascl, bscl;
X{
X result->x = (ascl * a->x) + (bscl * b->x);
X result->y = (ascl * a->y) + (bscl * b->y);
X return(result);
X };
X
X/* multiply two vectors together component-wise */
XVector2 *V2Mul (a, b, result)
XVector2 *a, *b, *result;
X{
X result->x = a->x * b->x;
X result->y = a->y * b->y;
X return(result);
X };
X
X/* return the distance between two points */
Xdouble V2DistanceBetween2Points(a, b)
XPoint2 *a, *b;
X{
Xdouble dx = a->x - b->x;
Xdouble dy = a->y - b->y;
X return(sqrt((dx*dx)+(dy*dy)));
X };
X
X/* return the vector perpendicular to the input vector a */
XVector2 *V2MakePerpendicular(a, ap)
XVector2 *a, *ap;
X{
X ap->x = -a->y;
X ap->y = a->x;
X return(ap);
X };
X
X/* create, initialize, and return a new vector */
XVector2 *V2New(x, y)
Xdouble x, y;
X{
XVector2 *v = NEWTYPE(Vector2);
X v->x = x; v->y = y;
X return(v);
X };
X
X
X/* create, initialize, and return a duplicate vector */
XVector2 *V2Duplicate(a)
XVector2 *a;
X{
XVector2 *v = NEWTYPE(Vector2);
X v->x = a->x; v->y = a->y;
X return(v);
X };
X
X/* multiply a point by a matrix and return the transformed point */
XPoint2 *V2MulPointByMatrix(p, m)
XPoint2 *p;
XMatrix3 *m;
X{
Xdouble w;
X p->x = (p->x * m->element[0][0]) +
X (p->y * m->element[1][0]) + m->element[2][0];
X p->y = (p->x * m->element[0][1]) +
X (p->y * m->element[1][1]) + m->element[2][1];
X w = (p->x * m->element[0][2]) +
X (p->y * m->element[1][2]) + m->element[2][2];
X if (w != 0.0) { p->x /= w; p->y /= w; }
X return(p);
X };
X
X/* multiply together matrices c = ab */
X/* note that c must not point to either of the input matrices */
XMatrix3 *V2MatMul(a, b, c)
XMatrix3 *a, *b, *c;
X{
Xint i, j, k;
X for (i=0; i<3; i++) {
X for (j=0; j<3; j++) {
X c->element[i][j] = 0;
X for (k=0; k<3; k++) c->element[i][j] +=
X a->element[i][k] * b->element[k][j];
X };
X };
X return(c);
X };
X
X
X
X
X/******************/
X/* 3d Library */
X/******************/
X
X/* returns squared length of input vector */
Xdouble V3SquaredLength(a)
XVector3 *a;
X{
X return((a->x * a->x)+(a->y * a->y)+(a->z * a->z));
X };
X
X/* returns length of input vector */
Xdouble V3Length(a)
XVector3 *a;
X{
X return(sqrt(V3SquaredLength(a)));
X };
X
X/* negates the input vector and returns it */
XVector3 *V3Negate(v)
XVector3 *v;
X{
X v->x = -v->x; v->y = -v->y; v->z = -v->z;
X return(v);
X };
X
X/* normalizes the input vector and returns it */
XVector3 *V3Normalize(v)
XVector3 *v;
X{
Xdouble len = V3Length(v);
X if (len != 0.0) { v->x /= len; v->y /= len; v->z /= len; };
X return(v);
X };
X
X/* scales the input vector to the new length and returns it */
XVector3 *V3Scale(v, newlen)
XVector3 *v;
Xdouble newlen;
X{
Xdouble len = V3Length(v);
X if (len != 0.0) {
X v->x *= newlen/len; v->y *= newlen/len; v->z *= newlen/len;
X };
X return(v);
X };
X
X
X/* return vector sum c = a+b */
XVector3 *V3Add(a, b, c)
XVector3 *a, *b, *c;
X{
X c->x = a->x+b->x; c->y = a->y+b->y; c->z = a->z+b->z;
X return(c);
X };
X
X/* return vector difference c = a-b */
XVector3 *V3Sub(a, b, c)
XVector3 *a, *b, *c;
X{
X c->x = a->x-b->x; c->y = a->y-b->y; c->z = a->z-b->z;
X return(c);
X };
X
X/* return the dot product of vectors a and b */
Xdouble V3Dot(a, b)
XVector3 *a, *b;
X{
X return((a->x*b->x)+(a->y*b->y)+(a->z*b->z));
X };
X
X/* linearly interpolate between vectors by an amount alpha */
X/* and return the resulting vector. */
X/* When alpha=0, result=lo. When alpha=1, result=hi. */
XVector3 *V3Lerp(lo, hi, alpha, result)
XVector3 *lo, *hi, *result;
Xdouble alpha;
X{
X result->x = LERP(alpha, lo->x, hi->x);
X result->y = LERP(alpha, lo->y, hi->y);
X result->z = LERP(alpha, lo->z, hi->z);
X return(result);
X };
X
X/* make a linear combination of two vectors and return the result. */
X/* result = (a * ascl) + (b * bscl) */
XVector3 *V3Combine (a, b, result, ascl, bscl)
XVector3 *a, *b, *result;
Xdouble ascl, bscl;
X{
X result->x = (ascl * a->x) + (bscl * b->x);
X result->y = (ascl * a->y) + (bscl * b->y);
X result->y = (ascl * a->z) + (bscl * b->z);
X return(result);
X };
X
X
X/* multiply two vectors together component-wise and return the result */
XVector3 *V3Mul (a, b, result)
XVector3 *a, *b, *result;
X{
X result->x = a->x * b->x;
X result->y = a->y * b->y;
X result->z = a->z * b->z;
X return(result);
X };
X
X/* return the distance between two points */
Xdouble V3DistanceBetween2Points(a, b)
XPoint3 *a, *b;
X{
Xdouble dx = a->x - b->x;
Xdouble dy = a->y - b->y;
Xdouble dz = a->z - b->z;
X return(sqrt((dx*dx)+(dy*dy)+(dz*dz)));
X };
X
X/* return the cross product c = a cross b */
XVector3 *V3Cross(a, b, c)
XVector3 *a, *b, *c;
X{
X c->x = (a->y*b->z) - (a->z*b->y);
X c->y = (a->z*b->x) - (a->x*b->z);
X c->z = (a->x*b->y) - (a->y*b->x);
X return(c);
X };
X
X/* create, initialize, and return a new vector */
XVector3 *V3New(x, y, z)
Xdouble x, y, z;
X{
XVector3 *v = NEWTYPE(Vector3);
X v->x = x; v->y = y; v->z = z;
X return(v);
X };
X
X/* create, initialize, and return a duplicate vector */
XVector3 *V3Duplicate(a)
XVector3 *a;
X{
XVector3 *v = NEWTYPE(Vector3);
X v->x = a->x; v->y = a->y; v->z = a->z;
X return(v);
X };
X
X
X/* multiply a point by a matrix and return the transformed point */
XPoint3 *V3MulPointByMatrix(p, m)
XPoint3 *p;
XMatrix4 *m;
X{
Xdouble w;
X p->x = (p->x * m->element[0][0]) + (p->y * m->element[1][0]) +
X (p->z * m->element[2][0]) + m->element[3][0];
X p->y = (p->x * m->element[0][1]) + (p->y * m->element[1][1]) +
X (p->z * m->element[2][1]) + m->element[3][1];
X p->z = (p->x * m->element[0][2]) + (p->y * m->element[1][2]) +
X (p->z * m->element[2][2]) + m->element[3][2];
X w = (p->x * m->element[0][3]) + (p->y * m->element[1][3]) +
X (p->z * m->element[2][3]) + m->element[3][3];
X if (w != 0.0) { p->x /= w; p->y /= w; p->z /= w; }
X return(p);
X };
X
X/* multiply together matrices c = ab */
X/* note that c must not point to either of the input matrices */
XMatrix4 *V3MatMul(a, b, c)
XMatrix4 *a, *b, *c;
X{
Xint i, j, k;
X for (i=0; i<4; i++) {
X for (j=0; j<4; j++) {
X c->element[i][j] = 0;
X for (k=0; k<4; k++) c->element[i][j] +=
X a->element[i][k] * b->element[k][j];
X };
X };
X return(c);
X };
X
X/* binary greatest common divisor by Silver and Terzian. See Knuth */
X/* both inputs must be >= 0 */
Xgcd(u, v)
Xint u, v;
X{
Xint k, t, f;
X if ((u<0) || (v<0)) return(1); /* error if u<0 or v<0 */
X k = 0; f = 1;
X while ((0 == (u%2)) && (0 == (v%2))) {
X k++; u>>=1; v>>=1, f*=2;
X };
X if (u&01) { t = -v; goto B4; } else { t = u; }
X B3: if (t > 0) { t >>= 1; } else { t = -((-t) >> 1); };
X B4: if (0 == (t%2)) goto B3;
X
X if (t > 0) u = t; else v = -t;
X if (0 != (t = u - v)) goto B3;
X return(u*f);
X };
X
X/***********************/
X/* Useful Routines */
X/***********************/
X
X/* return roots of ax^2+bx+c */
X/* stable algebra derived from Numerical Recipes by Press et al.*/
Xint quadraticRoots(a, b, c, roots)
Xdouble a, b, c, *roots;
X{
Xdouble d, q;
Xint count = 0;
X d = (b*b)-(4*a*c);
X if (d < 0.0) { *roots = *(roots+1) = 0.0; return(0); };
X q = -0.5 * (b + (SGN(b)*sqrt(d)));
X if (a != 0.0) { *roots++ = q/a; count++; }
X if (q != 0.0) { *roots++ = c/q; count++; }
X return(count);
X };
X
X
X/* generic 1d regula-falsi step. f is function to evaluate */
X/* interval known to contain root is given in left, right */
X/* returns new estimate */
Xdouble RegulaFalsi(f, left, right)
Xdouble (*f)(), left, right;
X{
Xdouble d = (*f)(right) - (*f)(left);
X if (d != 0.0) return (right - (*f)(right)*(right-left)/d);
X return((left+right)/2.0);
X };
X
X/* generic 1d Newton-Raphson step. f is function, df is derivative */
X/* x is current best guess for root location. Returns new estimate */
Xdouble NewtonRaphson(f, df, x)
Xdouble (*f)(), (*df)(), x;
X{
Xdouble d = (*df)(x);
X if (d != 0.0) return (x-((*f)(x)/d));
X return(x-1.0);
X };
X
X
X/* hybrid 1d Newton-Raphson/Regula Falsi root finder. */
X/* input function f and its derivative df, an interval */
X/* left, right known to contain the root, and an error tolerance */
X/* Based on Blinn */
Xdouble findroot(left, right, tolerance, f, df)
Xdouble left, right, tolerance;
Xdouble (*f)(), (*df)();
X{
Xdouble newx = left;
X while (ABS((*f)(newx)) > tolerance) {
X newx = NewtonRaphson(f, df, newx);
X if (newx < left || newx > right)
X newx = RegulaFalsi(f, left, right);
X if ((*f)(newx) * (*f)(left) <= 0.0) right = newx;
X else left = newx;
X };
X return(newx);
X };
END_OF_FILE
if test 10092 -ne `wc -c <'GGVecLib.c'`; then
echo shar: \"'GGVecLib.c'\" unpacked with wrong size!
fi
# end of 'GGVecLib.c'
fi
if test -f 'NearestPoint.c' -a "${1}" != "-c" ; then
echo shar: Will not clobber existing file \"'NearestPoint.c'\"
else
echo shar: Extracting \"'NearestPoint.c'\" \(12269 characters\)
sed "s/^X//" >'NearestPoint.c' <<'END_OF_FILE'
X/*
XSolving the Nearest Point-on-Curve Problem
Xand
XA Bezier Curve-Based Root-Finder
Xby Philip J. Schneider
Xfrom "Graphics Gems", Academic Press, 1990
X*/
X
X /* point_on_curve.c */
X
X#include <stdio.h>
X#include <malloc.h>
X#include <math.h>
X#include "GraphicsGems.h"
X
X#define TESTMODE
X
X/*
X * Forward declarations
X */
XPoint2 NearestPointOnCurve();
Xstatic int FindRoots();
Xstatic Point2 *ConvertToBezierForm();
Xstatic double ComputeXIntercept();
Xstatic int ControlPolygonFlatEnough();
Xstatic int CrossingCount();
Xstatic Point2 Bezier();
Xstatic Vector2 V2ScaleII();
X
Xint MAXDEPTH = 64; /* Maximum depth for recursion */
X
X#define EPSILON (ldexp(1.0,-MAXDEPTH-1)) /*Flatness control value */
X#define DEGREE 3 /* Cubic Bezier curve */
X#define W_DEGREE 5 /* Degree of eqn to find roots of */
X
X#ifdef TESTMODE
X/*
X * main :
X * Given a cubic Bezier curve (i.e., its control points), and some
X * arbitrary point in the plane, find the point on the curve
X * closest to that arbitrary point.
X */
Xmain()
X{
X
X static Point2 bezCurve[4] = { /* A cubic Bezier curve */
X { 0.0, 0.0 },
X { 1.0, 2.0 },
X { 3.0, 3.0 },
X { 4.0, 2.0 },
X };
X static Point2 arbPoint = { 3.5, 2.0 }; /*Some arbitrary point*/
X Point2 pointOnCurve; /* Nearest point on the curve */
X
X /* Find the closest point */
X pointOnCurve = NearestPointOnCurve(arbPoint, bezCurve);
X printf("pointOnCurve : (%4.4f, %4.4f)\n", pointOnCurve.x,
X pointOnCurve.y);
X}
X#endif /* TESTMODE */
X
X
X/*
X * NearestPointOnCurve :
X * Compute the parameter value of the point on a Bezier
X * curve segment closest to some arbtitrary, user-input point.
X * Return the point on the curve at that parameter value.
X *
X */
XPoint2 NearestPointOnCurve(P, V)
X Point2 P; /* The user-supplied point */
X Point2 *V; /* Control points of cubic Bezier */
X{
X Point2 *w; /* Ctl pts for 5th-degree eqn */
X double t_candidate[W_DEGREE]; /* Possible roots */
X int n_solutions; /* Number of roots found */
X double t; /* Parameter value of closest pt*/
X
X /* Convert problem to 5th-degree Bezier form */
X w = ConvertToBezierForm(P, V);
X
X /* Find all possible roots of 5th-degree equation */
X n_solutions = FindRoots(w, W_DEGREE, t_candidate, 0);
X free((char *)w);
X
X /* Compare distances of P to all candidates, and to t=0, and t=1 */
X {
X double dist, new_dist;
X Point2 p;
X Vector2 v;
X int i;
X
X
X /* Check distance to beginning of curve, where t = 0 */
X dist = V2SquaredLength(V2Sub(&P, &V[0], &v));
X t = 0.0;
X
X /* Find distances for candidate points */
X for (i = 0; i < n_solutions; i++) {
X p = Bezier(V, DEGREE, t_candidate[i], NULL, NULL);
X new_dist = V2SquaredLength(V2Sub(&P, &p, &v));
X if (new_dist < dist) {
X dist = new_dist;
X t = t_candidate[i];
X }
X }
X
X /* Finally, look at distance to end point, where t = 1.0 */
X new_dist = V2SquaredLength(V2Sub(&P, &V[DEGREE], &v));
X if (new_dist < dist) {
X dist = new_dist;
X t = 1.0;
X }
X }
X
X /* Return the point on the curve at parameter value t */
X printf("t : %4.12f\n", t);
X return (Bezier(V, DEGREE, t, NULL, NULL));
X}
X
X
X/*
X * ConvertToBezierForm :
X * Given a point and a Bezier curve, generate a 5th-degree
X * Bezier-format equation whose solution finds the point on the
X * curve nearest the user-defined point.
X */
Xstatic Point2 *ConvertToBezierForm(P, V)
X Point2 P; /* The point to find t for */
X Point2 *V; /* The control points */
X{
X int i, j, k, m, n, ub, lb;
X double t; /* Value of t for point P */
X int row, column; /* Table indices */
X Vector2 c[DEGREE+1]; /* V(i)'s - P */
X Vector2 d[DEGREE]; /* V(i+1) - V(i) */
X Point2 *w; /* Ctl pts of 5th-degree curve */
X double cdTable[3][4]; /* Dot product of c, d */
X static double z[3][4] = { /* Precomputed "z" for cubics */
X {1.0, 0.6, 0.3, 0.1},
X {0.4, 0.6, 0.6, 0.4},
X {0.1, 0.3, 0.6, 1.0},
X };
X
X
X /*Determine the c's -- these are vectors created by subtracting*/
X /* point P from each of the control points */
X for (i = 0; i <= DEGREE; i++) {
X V2Sub(&V[i], &P, &c[i]);
X }
X /* Determine the d's -- these are vectors created by subtracting*/
X /* each control point from the next */
X for (i = 0; i <= DEGREE - 1; i++) {
X d[i] = V2ScaleII(V2Sub(&V[i+1], &V[i], &d[i]), 3.0);
X }
X
X /* Create the c,d table -- this is a table of dot products of the */
X /* c's and d's */
X for (row = 0; row <= DEGREE - 1; row++) {
X for (column = 0; column <= DEGREE; column++) {
X cdTable[row][column] = V2Dot(&d[row], &c[column]);
X }
X }
X
X /* Now, apply the z's to the dot products, on the skew diagonal*/
X /* Also, set up the x-values, making these "points" */
X w = (Point2 *)malloc((unsigned)(W_DEGREE+1) * sizeof(Point2));
X for (i = 0; i <= W_DEGREE; i++) {
X w[i].y = 0.0;
X w[i].x = (double)(i) / W_DEGREE;
X }
X
X n = DEGREE;
X m = DEGREE-1;
X for (k = 0; k <= n + m; k++) {
X lb = MAX(0, k - m);
X ub = MIN(k, n);
X for (i = lb; i <= ub; i++) {
X j = k - i;
X w[i+j].y += cdTable[j][i] * z[j][i];
X }
X }
X
X return (w);
X}
X
X
X/*
X * FindRoots :
X * Given a 5th-degree equation in Bernstein-Bezier form, find
X * all of the roots in the interval [0, 1]. Return the number
X * of roots found.
X */
Xstatic int FindRoots(w, degree, t, depth)
X Point2 *w; /* The control points */
X int degree; /* The degree of the polynomial */
X double *t; /* RETURN candidate t-values */
X int depth; /* The depth of the recursion */
X{
X int i;
X Point2 Left[W_DEGREE+1], /* New left and right */
X Right[W_DEGREE+1]; /* control polygons */
X int left_count, /* Solution count from */
X right_count; /* children */
X double left_t[W_DEGREE+1], /* Solutions from kids */
X right_t[W_DEGREE+1];
X
X switch (CrossingCount(w, degree)) {
X case 0 : { /* No solutions here */
X return 0;
X break;
X }
X case 1 : { /* Unique solution */
X /* Stop recursion when the tree is deep enough */
X /* if deep enough, return 1 solution at midpoint */
X if (depth >= MAXDEPTH) {
X t[0] = (w[0].x + w[W_DEGREE].x) / 2.0;
X return 1;
X }
X if (ControlPolygonFlatEnough(w, degree)) {
X t[0] = ComputeXIntercept(w, degree);
X return 1;
X }
X break;
X }
X}
X
X /* Otherwise, solve recursively after */
X /* subdividing control polygon */
X Bezier(w, degree, 0.5, Left, Right);
X left_count = FindRoots(Left, degree, left_t, depth+1);
X right_count = FindRoots(Right, degree, right_t, depth+1);
X
X
X /* Gather solutions together */
X for (i = 0; i < left_count; i++) {
X t[i] = left_t[i];
X }
X for (i = 0; i < right_count; i++) {
X t[i+left_count] = right_t[i];
X }
X
X /* Send back total number of solutions */
X return (left_count+right_count);
X}
X
X
X/*
X * CrossingCount :
X * Count the number of times a Bezier control polygon
X * crosses the 0-axis. This number is >= the number of roots.
X *
X */
Xstatic int CrossingCount(V, degree)
X Point2 *V; /* Control pts of Bezier curve */
X int degree; /* Degreee of Bezier curve */
X{
X int i;
X int n_crossings = 0; /* Number of zero-crossings */
X int sign, old_sign; /* Sign of coefficients */
X
X sign = old_sign = SGN(V[0].y);
X for (i = 1; i <= degree; i++) {
X sign = SGN(V[i].y);
X if (sign != old_sign) n_crossings++;
X old_sign = sign;
X }
X return n_crossings;
X}
X
X
X
X/*
X * ControlPolygonFlatEnough :
X * Check if the control polygon of a Bezier curve is flat enough
X * for recursive subdivision to bottom out.
X *
X */
Xstatic int ControlPolygonFlatEnough(V, degree)
X Point2 *V; /* Control points */
X int degree; /* Degree of polynomial */
X{
X int i; /* Index variable */
X double *distance; /* Distances from pts to line */
X double max_distance_above; /* maximum of these */
X double max_distance_below;
X double error; /* Precision of root */
X Vector2 t; /* Vector from V[0] to V[degree]*/
X double intercept_1,
X intercept_2,
X left_intercept,
X right_intercept;
X double a, b, c; /* Coefficients of implicit */
X /* eqn for line from V[0]-V[deg]*/
X
X /* Find the perpendicular distance */
X /* from each interior control point to */
X /* line connecting V[0] and V[degree] */
X distance = (double *)malloc((unsigned)(degree + 1) * sizeof(double));
X {
X double abSquared;
X
X /* Derive the implicit equation for line connecting first *'
X /* and last control points */
X a = V[0].y - V[degree].y;
X b = V[degree].x - V[0].x;
X c = V[0].x * V[degree].y - V[degree].x * V[0].y;
X
X abSquared = (a * a) + (b * b);
X
X for (i = 1; i < degree; i++) {
X /* Compute distance from each of the points to that line */
X distance[i] = a * V[i].x + b * V[i].y + c;
X if (distance[i] > 0.0) {
X distance[i] = (distance[i] * distance[i]) / abSquared;
X }
X if (distance[i] < 0.0) {
X distance[i] = -((distance[i] * distance[i]) / abSquared);
X }
X }
X }
X
X
X /* Find the largest distance */
X max_distance_above = 0.0;
X max_distance_below = 0.0;
X for (i = 1; i < degree; i++) {
X if (distance[i] < 0.0) {
X max_distance_below = MIN(max_distance_below, distance[i]);
X };
X if (distance[i] > 0.0) {
X max_distance_above = MAX(max_distance_above, distance[i]);
X }
X }
X free((char *)distance);
X
X {
X double det, dInv;
X double a1, b1, c1, a2, b2, c2;
X
X /* Implicit equation for zero line */
X a1 = 0.0;
X b1 = 1.0;
X c1 = 0.0;
X
X /* Implicit equation for "above" line */
X a2 = a;
X b2 = b;
X c2 = c + max_distance_above;
X
X det = a1 * b2 - a2 * b1;
X dInv = 1.0/det;
X
X intercept_1 = (b1 * c2 - b2 * c1) * dInv;
X
X /* Implicit equation for "below" line */
X a2 = a;
X b2 = b;
X c2 = c + max_distance_below;
X
X det = a1 * b2 - a2 * b1;
X dInv = 1.0/det;
X
X intercept_2 = (b1 * c2 - b2 * c1) * dInv;
X }
X
X /* Compute intercepts of bounding box */
X left_intercept = MIN(intercept_1, intercept_2);
X right_intercept = MAX(intercept_1, intercept_2);
X
X error = 0.5 * (right_intercept-left_intercept);
X if (error < EPSILON) {
X return 1;
X }
X else {
X return 0;
X }
X}
X
X
X
X/*
X * ComputeXIntercept :
X * Compute intersection of chord from first control point to last
X * with 0-axis.
X *
X */
Xstatic double ComputeXIntercept(V, degree)
X Point2 *V; /* Control points */
X int degree; /* Degree of curve */
X{
X double XLK, YLK, XNM, YNM, XMK, YMK;
X double det, detInv;
X double S, T;
X double X, Y;
X
X XLK = 1.0 - 0.0;
X YLK = 0.0 - 0.0;
X XNM = V[degree].x - V[0].x;
X YNM = V[degree].y - V[0].y;
X XMK = V[0].x - 0.0;
X YMK = V[0].y - 0.0;
X
X det = XNM*YLK - YNM*XLK;
X detInv = 1.0/det;
X
X S = (XNM*YMK - YNM*XMK) * detInv;
X T = (XLK*YMK - YLK*XMK) * detInv;
X
X X = 0.0 + XLK * S;
X Y = 0.0 + YLK * S;
X
X return X;
X}
X
X
X/*
X * Bezier :
X * Evaluate a Bezier curve at a particular parameter value
X * Fill in control points for resulting sub-curves if "Left" and
X * "Right" are non-null.
X *
X */
Xstatic Point2 Bezier(V, degree, t, Left, Right)
X int degree; /* Degree of bezier curve */
X Point2 *V; /* Control pts */
X double t; /* Parameter value */
X Point2 *Left; /* RETURN left half ctl pts */
X Point2 *Right; /* RETURN right half ctl pts */
X{
X int i, j; /* Index variables */
X Point2 Vtemp[W_DEGREE+1][W_DEGREE+1];
X
X
X /* Copy control points */
X for (j =0; j <= degree; j++) {
X Vtemp[0][j] = V[j];
X }
X
X /* Triangle computation */
X for (i = 1; i <= degree; i++) {
X for (j =0 ; j <= degree - i; j++) {
X Vtemp[i][j].x =
X (1.0 - t) * Vtemp[i-1][j].x + t * Vtemp[i-1][j+1].x;
X Vtemp[i][j].y =
X (1.0 - t) * Vtemp[i-1][j].y + t * Vtemp[i-1][j+1].y;
X }
X }
X
X if (Left != NULL) {
X for (j = 0; j <= degree; j++) {
X Left[j] = Vtemp[j][0];
X }
X }
X if (Right != NULL) {
X for (j = 0; j <= degree; j++) {
X Right[j] = Vtemp[degree-j][j];
X }
X }
X
X return (Vtemp[degree][0]);
X}
X
Xstatic Vector2 V2ScaleII(v, s)
X Vector2 *v;
X double s;
X{
X Vector2 result;
X
X result.x = v->x * s; result.y = v->y * s;
X return (result);
X}
END_OF_FILE
if test 12269 -ne `wc -c <'NearestPoint.c'`; then
echo shar: \"'NearestPoint.c'\" unpacked with wrong size!
fi
# end of 'NearestPoint.c'
fi
echo shar: End of archive 5 \(of 5\).
cp /dev/null ark5isdone
MISSING=""
for I in 1 2 3 4 5 ; do
if test ! -f ark${I}isdone ; then
MISSING="${MISSING} ${I}"
fi
done
if test "${MISSING}" = "" ; then
echo You have unpacked all 5 archives.
rm -f ark[1-9]isdone
else
echo You still need to unpack the following archives:
echo " " ${MISSING}
fi
## End of shell archive.
exit 0
More information about the Comp.sources.misc
mailing list