text: #ifndef SimTK_SIMMATH_CONTACT_GEOMETRY_H_
  #define SimTK_SIMMATH_CONTACT_GEOMETRY_H_
  
  /* -------------------------------------------------------------------------- *
   *                        Simbody(tm): SimTKmath                              *
   * -------------------------------------------------------------------------- *
   * This is part of the SimTK biosimulation toolkit originating from           *
   * Simbios, the NIH National Center for Physics-Based Simulation of           *
   * Biological Structures at Stanford, funded under the NIH Roadmap for        *
   * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
   *                                                                            *
   * Portions copyright (c) 2008-12 Stanford University and the Authors.        *
   * Authors: Peter Eastman, Michael Sherman                                    *
   * Contributors: Ian Stavness, Andreas Scholz                                                              *
   *                                                                            *
   * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
   * not use this file except in compliance with the License. You may obtain a  *
   * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
   *                                                                            *
   * Unless required by applicable law or agreed to in writing, software        *
   * distributed under the License is distributed on an "AS IS" BASIS,          *
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
   * See the License for the specific language governing permissions and        *
   * limitations under the License.                                             *
   * -------------------------------------------------------------------------- */
  
  /** @file
  Defines the ContactGeometry class and its API-visible local subclasses for
  individual contact shapes. **/
  
  #include "SimTKcommon.h"
  #include "simmath/internal/common.h"
  #include "simmath/internal/OrientedBoundingBox.h"
  #include "simmath/internal/Geodesic.h"
  #include "simmath/internal/BicubicSurface.h"
  
  #include <cassert>
  
  namespace SimTK {
  
  /** @class SimTK::ContactGeometryTypeId
  This is a unique integer type for quickly identifying specific types of 
  contact geometry for fast lookup purposes. **/
  SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactGeometryTypeId);
  
  class ContactGeometryImpl;
  class OBBTreeNodeImpl;
  class OBBTree;
  class Plane;
  
  
  
  //==============================================================================
  //                             CONTACT GEOMETRY
  //==============================================================================
  /** A ContactGeometry object describes the shape of all or part of the boundary
  of a solid object, for the purpose of modeling with Simbody physical 
  effects that occur at the surface of that object, such as contact and 
  wrapping forces. Surfaces may be finite or infinite (e.g. a halfspace). 
  Surfaces may be smooth or discrete (polyhedral). Smooth surfaces are defined
  implicitly as f(P)=0 (P=[px,py,pz]), and optionally may provide a surface
  parameterization P=f(u,v). An implicit representation is valid for any P;
  parametric representations may have limited validity, singular points, or may
  be defined only in a local neighborhood.
  
  A variety of operators are implemented by each specific surface type. Some of
  these are designed to support efficient implementation of higher-level 
  algorithms that deal in pairs of interacting objects, such as broad- and
  narrow-phase contact and minimum-distance calculations.
  
  The idea here is to collect all the important knowledge about a particular
  kind of geometric shape in one place, adding operators as needed to support
  new algorithms from time to time. 
  
  All surfaces provide these operations:
    - find closest point to a given point
    - find intersection with a given ray
    - find most extreme point in a given direction
    - return the outward-facing surface normal at a point
    - generate a polygonal mesh that approximates the surface
    - return a unique integer id that may be used to quickly determine the 
      concrete type of a generic surface
  
  Finite surfaces provide
    - a bounding sphere that encloses the entire surface
    - a bounding volume hierarchy with tight-fitting leaf nodes containing
      only simple primitives
  
  Smooth surfaces provide
    - Min/max curvatures and directions
    - Calculate a geodesic between two points on the surface, or 
      starting at a point for a given direction and length
  
  Individual surface types generally support additional operations that may
  be used by specialized algorithms that know they are working with that 
  particular kind of surface. For example, an algorithm for determining 
  ellipsoid-halfspace contact is likely to take advantage of special properties
  of both surfaces.
  
  We do not require
  detailed solid geometry, but neither can the surface be treated without some
  information about the solid it bounds. For example, for contact we must know
  which side of the surface is the "inside". However, we don't need a fully
  consistent treatment of the solid; for ease of modeling we require only that
  the surface behave properly in those locations at which it is evaluated at run
  time. The required behavior may vary depending on the algorithm using it.
  
  This is the base class for surface handles; user code will typically 
  reference one of the local classes it defines instead for specific shapes. **/
  class SimTK_SIMMATH_EXPORT ContactGeometry {
  public:
  class HalfSpace;
  class Sphere;
  class Ellipsoid;
  class Torus;
  class SmoothHeightMap;
  class Cylinder;
  class Brick;
  class TriangleMesh;
  
  // TODO
  class Cone;
  
  /** Base class default constructor creates an empty handle. **/
  ContactGeometry() : impl(0) {}
  /** Copy constructor makes a deep copy. **/
  ContactGeometry(const ContactGeometry& src);
  /** Copy assignment makes a deep copy. **/
  ContactGeometry& operator=(const ContactGeometry& src);
  /** Base class destructor deletes the implementation object.\ Note that this
  is not virtual; handles should consist of just a pointer to the 
  implementation. **/
  ~ContactGeometry();
  
  /** Generate a DecorativeGeometry that matches the shape of this ContactGeometry **/
  DecorativeGeometry createDecorativeGeometry() const;
  
  /** Given a point, find the nearest point on the surface of this object. If 
  multiple points on the surface are equally close to the specified point, this 
  may return any of them.
  @param[in]  position    The point in question.
  @param[out] inside      On exit, this is set to true if the specified point is 
                          inside this object, false otherwise.
  @param[out] normal      On exit, this contains the surface normal at the 
                          returned point.
  @return The point on the surface of the object which is closest to the 
  specified point. **/
  Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
  
  /** Given a query point Q, find the nearest point P on the surface of this 
  object, looking only down the local gradient. Thus we cannot guarantee that P
  is the globally nearest point; if you need that use the findNearestPoint()
  method. However, this method is extremely fast since it only needs to find the
  locally nearest point. It is best suited for use when you know P is not too
  far from the surface.
  
  @param[in]  pointQ      The query point Q, assumed to be somewhere not too far
                              from the surface.
  @return A point P on the surface, at which the surface normal is aligned with
  the line from P to Q. 
  
  This method is very cheap if query point Q is already on the surface to within
  a very tight tolerance; in that case it will simply return P=Q. **/
  Vec3 projectDownhillToNearestPoint(const Vec3& pointQ) const;
  
  /** Track the closest point between this implicit surface and a given line, or
  the point of deepest penetration if the line intersects the surface.
  We are given a guess at the closest point, and search only downhill from that
  guess so we can't guarantee we actually are returning the globally closest.
  However, the method does run very fast and is well suited to continuous 
  tracking where nothing dramatic happens from call to call. 
  
  If the line intersects the surface, we return the closest perpendicular point,
  \e not one of the intersection points. The perpendicular point will be the
  point of \e most separation rather than least. This behavior makes the 
  signed separation distance a smooth function that passes through zero as the
  line approaches, contacts, and penetrates the surface. We return that signed
  distance as the \a height argument.
  
  @param[in]  pointOnLine     Any point through which the line passes.
  @param[in]  directionOfLine A unit vector giving the direction of the line.
  @param[in]  startingGuessForClosestPoint
      A point on the implicit surface that is a good guess for the closest
      (or most deeply penetrated) point. 
  @param[out] newClosestPointOnSurface
      This is the point of least distance to the line if the surface and line are
      separated; the point of most distance to the line if the line intersects
      the surface.
  @param[out] closestPointOnLine
      The is the corresponding point on the line that is the closest (furthest)
      from \a newClosestPointOnSurface.
  @param[out] height
      This is the signed height of the closest point on the line over the 
      surface tangent plane at \a newClosestPointOnSurface. This is positive 
      when \a closestPointOnLine is in the direction of the outward normal, 
      negative if it is in the opposite direction. If we successfully found the
      point we sought, a negative height means the extreme point on the line
      is inside the object bounded by this surface.
  
  @returns \c true if it succeeds in finding a point that meets the criteria to
  a strict tolerance. Otherwise the method has gotten stuck in a local minimum
  meaning the initial guess wasn't good enough.
  
  In case we fail to find a good point, we'll still return \e some points on the
  surface and the line that reduced the error function. Sometimes those are
  useful for visualizing what went wrong.
  
  <h3>Theory</h3>
  We are looking for a point P on the surface where a line N passing through P
  parallel to the normal at P intersects the given line L and is perpendicular to
  L there. Thus there are two degrees of freedom (location of P on the
  surface), and there are two equations to solve. Let Q and R be the closest
  approach points of the lines N and L respectively. We require that the following
  two conditions hold:
    - lines N and L are perpendicular 
    - points Q and R are coincident
  
  To be precise we solve the following nonlinear system of three equations:
  <pre>
  err(P) = 0
  where
  err(P) = [     n . l       ]
           [ (R-Q) . (n X l) ]
           [      f(P)       ]
  In the above n is the unit normal vector at P, l is a unit vector aligned with
  the query line L, and f(P) is the implicit surface function that keeps P on the
  surface.
  </pre>
  **/
  bool trackSeparationFromLine(const Vec3& pointOnLine,
                               const UnitVec3& directionOfLine,
                               const Vec3& startingGuessForClosestPoint,
                               Vec3& newClosestPointOnSurface,
                               Vec3& closestPointOnLine,
                               Real& height) const;
  
  
  
  /** Determine whether this object intersects a ray, and if so, find the 
  intersection point.
  @param[in]  origin      The position at which the ray begins.
  @param[in]  direction   The ray direction.
  @param[out] distance    If an intersection is found, the distance from the ray 
                          origin to the intersection point is stored in this. 
                          Otherwise, it is left unchanged.
  @param[out] normal      If an intersection is found, the surface normal of the
                          intersection point is stored in this. Otherwise, it is 
                          left unchanged.
  @return \c true if an intersection is found, \c false otherwise. **/
  bool intersectsRay(const Vec3& origin, const UnitVec3& direction, 
                     Real& distance, UnitVec3& normal) const;
  
  /** Get a bounding sphere which completely encloses this object.
  @param[out] center  On exit, this contains the location of the center of the 
                      bounding sphere.
  @param[out] radius  On exit, this contains the radius of the bounding 
                      sphere. **/
  void getBoundingSphere(Vec3& center, Real& radius) const;
  
  /** Returns \c true if this is a smooth surface, meaning that it can provide
  meaningful curvature information and continuous derivatives with respect to its
  parameterization. **/
  bool isSmooth() const;
  
  /** Compute the principal curvatures and their directions, and the surface 
  normal, at a given point on a smooth surface.
  @param[in]      point        
      A point at which to compute the curvature.
  @param[out]     curvature    
      On return, this will contain the maximum (curvature[0]) and minimum 
      (curvature[1]) curvatures of the surface at the point.
  @param[out]     orientation  
      On return, this will contain the orientation of the surface at the given
      point as follows: the x axis along the direction of maximum curvature, the 
      y axis along the direction of minimum curvature, and the z axis along the 
      surface normal. These vectors are expressed in the surface's coordinate 
      frame.
  
  Non-smooth surfaces will not implement this method and will throw an exception
  if you call it. **/
  void calcCurvature(const Vec3& point, Vec2& curvature, 
                     Rotation& orientation) const;
  
  /** Our smooth surfaces define a function f(P)=0 that provides an implicit 
  representation of the surface. P=(x,y,z) is any point in space expressed in 
  the surface's coordinate frame S (that is, given by a vector P-So, expressed in
  S). The function is positive inside the object, 0 on the surface, and negative 
  outside the object. The returned Function object supports first and second 
  partial derivatives with respect to the three function arguments x, y, and z.
  Evaluation of the function and its derivatives is cheap. 
  
  Non-smooth surfaces will not implement this method and will throw an exception
  if you call it. **/
  const Function& getImplicitFunction() const;
  
  /** Calculate the value of the implicit surface function, at a given point.
  @param[in]      point
      A point at which to compute the surface value.
  @return
      The value of the implicit surface function at the point. **/
  Real calcSurfaceValue(const Vec3& point) const;
  
  /** Calculate the implicit surface outward facing unit normal at the given
  point. This is determined using the implicit surface function gradient
  so is undefined if the point is at a singular point of the implicit function.
  An example is a point along the center line of
  a cylinder. Rather than return a NaN unit normal in these cases, which
  would break many algorithms that are searching around for valid points, we'll
  return the normal from a nearby, hopefully non-singular point. If that doesn't
  work, we'll return an arbitrary direction. If you want to know whether you're
  at a singular point, obtain the gradient directly with calcSurfaceGradient()
  and check its length. **/
  UnitVec3 calcSurfaceUnitNormal(const Vec3& point) const;
  
  /** Calculate the gradient of the implicit surface function, at a given point.
  @param[in]      point
      A point at which to compute the surface gradient.
  @return
      The gradient of the implicit surface function at the point. **/
  Vec3 calcSurfaceGradient(const Vec3& point) const;
  
  /** Calculate the hessian of the implicit surface function, at a given point.
  @param[in]      point
      A point at which to compute the surface Hessian.
  @return
      The Hessian of the implicit surface function at the point. **/
  Mat33 calcSurfaceHessian(const Vec3& point) const;
  
  /** For an implicit surface, return the Gaussian curvature at the point p whose
  implicit surface function gradient g(p) and Hessian H(p) are supplied. It is
  not required that p is on the surface but the results will be meaningless if
  the gradient and Hessian were not calculated at the same point.
  
  Here is the formula:
  <pre>
          ~grad(f) * Adjoint(H) * grad(f)
     Kg = --------------------------------
                  |grad(f)|^4
  </pre>
  where grad(f) is Df/Dx, Hessian H is D grad(f)/Dx and Adjoint is a 3x3
  matrix A where A(i,j)=determinant(H with row i and column j removed). 
  Ref: Goldman, R. "Curvature formulas for implicit curves and surfaces",
  Comp. Aided Geometric Design 22 632-658 (2005).
  
  Gaussian curvature is the product of the two principal curvatures, Kg=k1*k2.
  So for example, the Gaussian curvature anywhere on a sphere is 1/r^2. Note
  that despite the name, Gaussian curvature has units of 1/length^2 rather than
  curvature units of 1/length.
  
  Here is what the (symmetric) adjoint matrix looks like:
  <pre>
  adjH  =  [ fyy*fzz - fyz^2, fxz*fyz - fxy*fzz, fxy*fyz - fxz*fyy  ]
           [      (1,2),      fxx*fzz - fxz^2,   fxy*fxz - fxx*fyz  ]
           [      (1,3),           (2,3),        fxx*fyy - fxy^2    ]
  </pre>
  **/
  Real calcGaussianCurvature(const Vec3&  gradient,
                             const Mat33& Hessian) const;
  
  /** This signature is for convenience; use the other one to save time if you
  already have the gradient and Hessian available for this point. See the other
  signature for documentation. **/
  Real calcGaussianCurvature(const Vec3& point) const {
      return calcGaussianCurvature(calcSurfaceGradient(point),
                                   calcSurfaceHessian(point)); 
  }
  
  /** For an implicit surface, return the curvature k of the surface at a given
  point p in a given direction tp. Make sure the point is on the surface and the 
  direction vector lies in the tangent plane and has unit length |tp| = 1. Then
  <pre>
  k = ~tp * H * tp / |g|,
  </pre>
  where H is the Hessian matrix evaluated at p. 
  **/
  Real calcSurfaceCurvatureInDirection(const Vec3& point, const UnitVec3& direction) const;
  
  /** For an implicit surface at a given point p, return the principal 
  curvatures and principal curvature directions, using only the implicit
  function and its derivatives. The curvatures are returned as a Vec2 (kmax,kmin), 
  and the directions are returned as the x,y axes of a frame whose z axis is the 
  outward unit normal at p, x is the maximum curvature direction, and y is the 
  minimum curvature direction. Point p is given in the surface frame S, and the 
  returned axes are given in S via the Rotation matrix R_SP.
  **/
  void calcSurfacePrincipalCurvatures(const Vec3& point, Vec2& curvature, 
                                      Rotation& R_SP) const;
  
  /** Returns \c true if this surface is known to be convex. This can be true
  for smooth or polygonal surfaces. **/
  bool isConvex() const;
  
  /** Given a direction expressed in the surface's frame S, return the point P on 
  the surface that is the furthest in that direction (or one of those points if
  there is more than one). This will be the point such that dot(P-So, direction)
  is maximal for the surface (where So is the origin of the surface). This is 
  particularly useful for convex surfaces and should be very fast for them. **/
  Vec3 calcSupportPoint(UnitVec3 direction) const;
  
  /** ContactTrackerSubsystem uses this id for fast identification of specific
  surface shapes. **/
  ContactGeometryTypeId getTypeId() const;
  
  /** Calculate surface curvature at a point using differential geometry as 
  suggested by Harris 2006, "Curvature of ellipsoids and other surfaces" Ophthal.
  Physiol. Opt. 26:497-501, although the equations here come directly from 
  Harris' reference Struik 1961, Lectures on Classical Differential Geometry, 
  2nd ed. republished by Dover 1988. Equation and page numbers below are from 
  Struik.
  
  This method works for any smooth surface for which there is a local (u,v) 
  surface parameterization; it is not restricted to ellipsoids or convex shapes, 
  and (u,v) must be distinct but do not have to be perpendicular. Both must be 
  perpendicular to the surface normal.
  
  First fundamental form:  I  = E du^2 + 2F dudv + G dv^2
  <pre>   E = dxdu^2, F = ~dxdu * dxdv, G=dxdv^2  </pre>
  
  Second fundamental form: II = e du^2 + 2f dudv + g dv^2
  <pre>   e = - ~d2xdu2 * nn, f = - ~d2xdudv * nn, g = - ~d2xdv2 * nn </pre>
  
  Given a direction t=dv/du, curvature k is
  <pre>
           II   e + 2f t + g t^2
       k = -- = ----------------   (eq. 6-3)
           I    E + 2F t + G t^2
  </pre>
  
  We want minimum and maximum values for k to get principal curvatures. We can 
  find those as the solutions to dk/dt=0.
  <pre>   dk/dt = (E + 2Ft + Gt^2)(f+gt) - (e + 2ft + gt^2)(F+Gt) </pre>
  
  When dk/dt=0, k =(f+gt)/(F+Gt) = (e+ft)/(E+Ft) (eq. 6-4). That provides a 
  quadratic equation for the two values of t:
  <pre>   A t^2 + B t + C = 0     </pre>
  where A=Fg-Gf, B=Eg-Ge, C=Ef-Fe  (eq. 6-5a).
  
  In case the u and v tangent directions are the min and max curvature directions
  (on a sphere, for example), they must be perpendicular so F=f=0 (eq. 6-6). Then
  the curvatures are ku = e/E and kv = g/G (eq. 6-8).
  
  We're going to return principal curvatures kmax and kmin such that kmax >= kmin,
  along with the perpendicular tangent unit directions dmax,dmin that are the 
  corresponding principal curvature directions, oriented so that (dmax,dmin,nn) 
  form a right-handed coordinate frame.
  
  Cost: given a point P, normalized normal nn, unnormalized u,v tangents and 
  second derivatives <pre>
      curvatures: ~115 flops
      directions:  ~50 flops
                  ----
                  ~165 flops
  </pre>  **/
  static Vec2 evalParametricCurvature(const Vec3& P, const UnitVec3& nn,
                                      const Vec3& dPdu, const Vec3& dPdv,
                                      const Vec3& d2Pdu2, const Vec3& d2Pdv2, 
                                      const Vec3& d2Pdudv,
                                      Transform& X_EP);
  
  /** This utility method is useful for characterizing the relative geometry of
  two locally-smooth surfaces in contact, in a way that is useful for later
  application of Hertz compliant contact theory for generating forces. We assume
  that contact points Q1 on surface1 and Q2 on surface2 have been determined with
  the following properties:
      - the surface normals are aligned but opposite
      - points Q1 and Q2 are separated only along the normal (no tangential 
        separation)
  
  Then the local regions near Q1 and Q2 may be fit with paraboloids P1 and P2
  that have their origins at Q1 and Q2, and have the same normals and curvatures
  at the origins as do the original surfaces. We will behave here as though
  Q1 and Q2 are coincident in space at a point Q; imagine sliding them along
  the normal until that happens. Now we define the equations of P1 and P2 in
  terms of the maximum and minimum curvatures of surface1 and surface2 at Q:<pre>
      P1: -2z = kmax1 x1^2 + kmin1 y1^2
      P2:  2z = kmax2 x2^2 + kmin2 y2^2  
  </pre>
  Although the origin Q and z direction are shared, the x,y directions for the 
  two paraboloids, though in the same plane z=0, are relatively rotated. Note
  that the kmins might be negative; the surfaces do not have to be convex. 
  
  For Hertz contact, we need to know the difference (relative) surface
  between the two paraboloids. The difference is a paraboloid P with equation
  <pre>
      P: -2z = kmax x^2 + kmin y^2
  </pre>
  It shares the origin Q and z direction (oriented as for P1), but has its
  own principal directions x,y which are coplanar with x1,y1 and x2,y2 but
  rotated into some unknown composite orientation. The purpose of this method
  is to calculate kmax and kmin, and optionally (depending which signature you
  call), x and y, the directions of maximum and minimum curvature (resp.). The
  curvature directions are also the principal axes of the contact ellipse formed
  by the deformed surfaces, so are necessary (for example) if you want to draw
  that ellipse.
  
  Cost is about 220 flops. If you don't need the curvature directions, call the
  other overloaded signature which returns only kmax and kmin and takes only 
  about 1/3 as long. 
  
  @param[in]          R_SP1  
      The orientation of the P1 paraboloid's frame, expressed in some frame S 
      (typically the frame of the surface to which P1 is fixed). R_SP1.x() is 
      the direction of maximum curvature; y() is minimum curvature; z is the
      contact normal pointing away from surface 1.
  @param[in]          k1      
      The maximum (k1[0]) and minimum (k1[1]) curvatures for P1 at the contact 
      point Q1 on surface1. Negative curvatures are handled correctly here but
      may cause trouble for your force model if the resulting contact is 
      conforming.
  @param[in]          x2      
      The direction of maximum curvature for paraboloid P2. \a x2 must be in the 
      x1,y1 plane provided in \a R_SP1 and expressed in the S frame.
  @param[in]          k2      
      The maximum (k2[0]) and minimum (k2[1]) curvatures for P2 at the contact 
      point Q2 on surface2. Negative curvatures are handled correctly here but
      may cause trouble for your force model if the resulting contact is 
      conforming.
  @param[out]         R_SP    
      The orientation of the difference paraboloid P's frame, expressed in the 
      same S frame as was used for P1. R_SP.x() is the direction of maximum 
      curvature of P at the contact point; y() is the minimum curvature 
      direction; z() is the unchanged contact normal pointing away from surface1.
  @param[out]         k       
      The maximum (k[0]) and minimum(k[1]) curvatures for the difference 
      paraboloid P at the contact point Q. If either of these is negative or
      zero then the surfaces are conforming and you can't use a point contact
      force model. Note that if k1>0 and k2>0 (i.e. surfaces are convex at Q)
      then k>0 too. If some of the surface curvatures are concave, it is still
      possible that k>0, depending on the relative curvatures.
  
  @see The other signature for combineParaboloids() that is much cheaper if
  you just need the curvatures \a k but not the directions \a R_SP. **/
  static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
                                 const UnitVec3& x2, const Vec2& k2,
                                 Rotation& R_SP, Vec2& k);
  
  /** This is a much faster version of combineParaboloids() for when you just
  need the curvatures of the difference paraboloid, but not the directions 
  of those curvatures. Cost is about 70 flops. See the other overload of
  this method for details. **/
  static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
                                 const UnitVec3& x2, const Vec2& k2,
                                 Vec2& k);
  
  
  /** @name                  Geodesic Evaluators **/
  /**@{**/
  
  /** Given two points, find a geodesic curve connecting them.
  If a preferred starting point is provided, find the geodesic curve that
  is closest to that point. Otherwise, find the shortest length geodesic.
  
  @param[in]  xP          Coordinates of starting point P, in S.
  @param[in]  xQ          Coordinates of ending point Q, in S.
  @param[in] xSP           (Optional) Coordinates of a preferred point for the geodesic to be near
  @param[in] options       Parameters related to geodesic calculation
  @param[out] geod         On exit, this contains a geodesic between P and Q.
  **/
  void initGeodesic(const Vec3& xP, const Vec3& xQ, const Vec3& xSP,
          const GeodesicOptions& options, Geodesic& geod) const;
  
  
  /** Given the current positions of two points P and Q moving on this surface, 
  and the previous geodesic curve G' connecting prior locations P' and Q' of
  those same two points, return the geodesic G between P and Q that is closest in
  length to the previous one. If multiple equal-length geodesics are possible
  (rare; between poles of a sphere, for example) then the one best matching the 
  direction of the previous geodesic is selected.
  
  @param[in]  xP          Coordinates of starting point P, in S.
  @param[in]  xQ          Coordinates of ending point Q, in S.
  @param[in]  prevGeod    The previous geodesic to which the new one should be
                              similar.
  @param[in]  options     Parameters controlling the computation of the
                              new geodesic.
  @param[out] geod        On exit, this contains a geodesic between P and Q.
  
  <h3>Algorithm</h3>
  The handling of continuity here enforces our desire to have the length of a 
  geodesic change smoothly as its end points move over the surface. This also
  permits us to accelerate geodesic finding by using starting guesses that are
  extrapolated from the previous result. If we find that the new geodesic has
  changed length substantially from the previous one, we will flag the result
  as uncertain and the caller should reduce the integration step size.
  
  First, classify the previous geodesic G' as "direct" or "indirect". Direct 
  means that both tangents tP' and tQ' are approximately aligned with the vector
  r_PQ'. Note that as points P and Q get close together, as occurs prior to 
  a cable liftoff, the geodesic between them always becomes direct and in fact 
  just prior to liftoff it is the straight line from P to Q.
  
  If G' was indirect, then G is the geodesic connecting P and Q that has tP 
  closest to tP' and about the same length as G'. We use G' data to initialize
  our computation of G and perform a local search to find the closest match.
  
  If G' was direct, then we look at the direction of r_PQ. If it is still aligned
  with the previous tP', then we calculate G from P to Q starting in direction
  tP', with roughly length s'. If it is anti-aligned, P and Q have swapped
  positions, presumably because they should have lifted off. In that case
  we calculate G from P to Q in direction -tP', and report that the geodesic
  has flipped. In that case you can consider the length s to be negative and
  use the value of s as an event witness for liftoff events.
  
  **/
  // XXX if xP and xQ are the exact end-points of prevGeod; then geod = prevGeod;
  void continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod,
          const GeodesicOptions& options, Geodesic& geod) const;
  
  /** Produce a straight-line approximation to the (presumably short) geodesic 
  between two points on this implicit surface. We do not check here whether it
  is reasonable to treat this geodesic as a straight line; we assume the caller
  has made that determination.
  
  We are given points P and Q and choose
  P' and Q' as the nearest downhill points on the surface to the given points. Then the
  returned geodesic line runs from P' to Q'. The Geodesic object will contain only
  the start and end points of the geodesic, with all the necessary information
  filled in. The normals nP and nQ are calculated from the surface points P' and
  Q'. The binormal direction is calculated using a preferred direction vector d
  (see below) as bP=normalize(d X nP) and bQ=normalize(d X nQ). Then the tangents
  are tP=nP X bP and tQ=nQ X bQ.
  
  The preferred direction d is calculated as follows: if P' and Q' are 
  numerically indistinguishable (as defined by 
  Geo::Point::pointsAreNumericallyCoincident()), we'll use the given 
  \a defaultDirection if there is one, otherwise d is an arbitrary 
  perpendicular to nP. If P' and Q' are numerically distinguishable, we 
  instead set d = normalize(Q-P). 
  
  When P' and Q' are \e numerically coincident, we will shift both of them to
  the midpoint (P'+Q')/2 so that the geodesic end points become \e exactly
  coincident and the resulting geodesic has exactly zero length. There will
  still be two points returned in the Geodesic object but they will be 
  identical. **/
  void makeStraightLineGeodesic(const Vec3& xP, const Vec3& xQ,
          const UnitVec3& defaultDirectionIfNeeded,
          const GeodesicOptions& options, Geodesic& geod) const;
  
  
  /** Compute a geodesic curve starting at the given point, starting in the
   * given direction, and terminating at the given length.
  
  @param[in] xP            Coordinates of the starting point for the geodesic.
  @param[in] tP            The starting tangent direction for the geodesic.
  @param[in] terminatingLength   The length that the resulting geodesic should have.
  @param[in] options       Parameters related to geodesic calculation
  @param[out] geod         On exit, this contains the calculated geodesic
  **/
  // XXX what to do if tP is not in the tangent plane at P -- project it?
  void shootGeodesicInDirectionUntilLengthReached
     (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength, 
      const GeodesicOptions& options, Geodesic& geod) const;
  
  /** Given an already-calculated geodesic on this surface connecting points
  P and Q, fill in the sensitivity of point P with respect to a change of
  tangent direction at Q. If there are interior points stored with the geodesic,
  then we'll calculate the interior sensitivities also.
  
  @param[in,out]  geodesic        An already-calculated geodesic.
  @param[in]      initSensitivity
      Initial conditions for the Jacobi field calculation. If this is the whole
      geodesic then the initial conditions are (0,1) for the sensitivity and
      its arc length derivative. However, if we are continuing from another
      geodesic, then the end sensitivity for that geodesic is the initial 
      conditions for this one. 
  **/
  void calcGeodesicReverseSensitivity
     (Geodesic& geodesic,
      const Vec2& initSensitivity = Vec2(0,1)) const; // j, jdot at end point
  
  
  /** Compute a geodesic curve starting at the given point, starting in the
   * given direction, and terminating when it hits the given plane.
  
  @param[in] xP            Coordinates of the starting point for the geodesic.
  @param[in] tP            The starting tangent direction for the geodesic.
  @param[in] terminatingPlane   The plane in which the end point of the resulting geodesic should lie.
  @param[in] options       Parameters related to geodesic calculation
  @param[out] geod         On exit, this contains the calculated geodesic
  **/
  // XXX what to do if tP is not in the tangent plane at P -- project it?
  // XXX what to do if we don't hit the plane
  void shootGeodesicInDirectionUntilPlaneHit(const Vec3& xP, const UnitVec3& tP,
          const Plane& terminatingPlane, const GeodesicOptions& options,
          Geodesic& geod) const;
  
  
  /** Utility method to find geodesic between P and Q using split geodesic 
  method with initial shooting directions tPhint and -tQhint. **/
  void calcGeodesic(const Vec3& xP, const Vec3& xQ,
          const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
  
  /** Utility method to find geodesic between P and Q using the orthogonal
  method, with initial direction tPhint and initial length lengthHint. **/
  void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ,
          const Vec3& tPhint, Real lengthHint, Geodesic& geod) const;
  
  /** This signature makes a guess at the initial direction and length
  and then calls the other signature. **/
  void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ,
          Geodesic& geod) const
  {
      const Vec3 r_PQ = xQ - xP;
      const Real lengthHint = r_PQ.norm();
      const UnitVec3 n = calcSurfaceUnitNormal(xP);
      // Project r_PQ into the tangent plane.
      const Vec3 t_PQ = r_PQ - (~r_PQ*n)*n;
      const Real tLength = t_PQ.norm();
      const UnitVec3 tPhint =
          tLength != 0 ? UnitVec3(t_PQ/tLength, true)
                       : n.perp(); // some arbitrary perpendicular to n
      calcGeodesicUsingOrthogonalMethod(xP, xQ, Vec3(tPhint), lengthHint, geod);           
  }
  
  
  /**
   * Utility method to calculate the "geodesic error" between one geodesic
   * shot from P in the direction tP and another geodesic shot from Q in the
   * direction tQ. We optionally return the resulting "kinked" geodesic in
   * case anyone wants it; if the returned error is below tolerance then that
   * geodesic is the good one.
   **/
  Vec2 calcSplitGeodError(const Vec3& P, const Vec3& Q,
                     const UnitVec3& tP, const UnitVec3& tQ,
                     Geodesic* geod=0) const;
  
  
  
  /** Analytically compute a geodesic curve starting at the given point, starting in the
   * given direction, and terminating at the given length. Only possible for a few simple
   * shapes, such as spheres and cylinders.
  
  @param[in] xP            Coordinates of the starting point for the geodesic.
  @param[in] tP            The starting tangent direction for the geodesic.
  @param[in] terminatingLength   The length that the resulting geodesic should have.
  @param[in] options       Parameters related to geodesic calculation
  @param[out] geod         On exit, this contains the calculated geodesic
  **/
  // XXX what to do if tP is not in the tangent plane at P -- project it?
  void shootGeodesicInDirectionUntilLengthReachedAnalytical
     (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength,
      const GeodesicOptions& options, Geodesic& geod) const;
  
  
  /** Analytically compute a geodesic curve starting at the given point, starting in the
   * given direction, and terminating when it hits the given plane. Only possible
   * for a few simple shapes, such as spheres and cylinders.
  
  @param[in] xP            Coordinates of the starting point for the geodesic.
  @param[in] tP            The starting tangent direction for the geodesic.
  @param[in] terminatingPlane   The plane in which the end point of the resulting geodesic should lie.
  @param[in] options       Parameters related to geodesic calculation
  @param[out] geod         On exit, this contains the calculated geodesic
  **/
  // XXX what to do if tP is not in the tangent plane at P -- project it?
  // XXX what to do if we don't hit the plane
  void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
          const Plane& terminatingPlane, const GeodesicOptions& options,
          Geodesic& geod) const;
  
  
  /** Utility method to analytically find geodesic between P and Q with initial shooting
   directions tPhint and tQhint. Only possible for a few simple shapes, such as spheres
   and cylinders.
   **/
  void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
          const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
  
  /**
   * Utility method to analytically calculate the "geodesic error" between one geodesic
   * shot from P in the direction tP and another geodesic shot from Q in the
   * direction tQ. We optionally return the resulting "kinked" geodesic in
   * case anyone wants it; if the returned error is below tolerance then that
   * geodesic is the good one. Only possible for a few simple shapes, such as spheres
   and cylinders.
   **/
  Vec2 calcSplitGeodErrorAnalytical(const Vec3& P, const Vec3& Q,
                     const UnitVec3& tP, const UnitVec3& tQ,
                     Geodesic* geod=0) const;
  
  /**@}**/
  
  
  /** @name Geodesic-related Debugging **/
  /**@{**/
  
  
  
  /** Get the plane associated with the
      geodesic hit plane event handler  **/
  const Plane& getPlane() const;
  /** Set the plane associated with the
      geodesic hit plane event handler  **/
  void setPlane(const Plane& plane) const;
  /** Get the geodesic for access by visualizer **/
  const Geodesic& getGeodP() const;
  /** Get the geodesic for access by visualizer **/
  const Geodesic& getGeodQ() const;
  const int getNumGeodesicsShot() const;
  void addVizReporter(ScheduledEventReporter* reporter) const;
  /**@}**/
  
  
  
  explicit ContactGeometry(ContactGeometryImpl* impl); /**< Internal use only. **/
  bool isOwnerHandle() const;                          /**< Internal use only. **/
  bool isEmptyHandle() const;                          /**< Internal use only. **/
  bool hasImpl() const {return impl != 0;}             /**< Internal use only. **/
  /** Internal use only. **/
  const ContactGeometryImpl& getImpl() const {assert(impl); return *impl;}
  /** Internal use only. **/
  ContactGeometryImpl& updImpl() {assert(impl); return *impl; }
  
  protected:
  ContactGeometryImpl* impl; /**< Internal use only. **/
  };
  
  
  
  //==============================================================================
  //                                 HALF SPACE
  //==============================================================================
  /** This ContactGeometry subclass represents an object that occupies the 
  entire half-space x>0. This is useful for representing walls and floors. This
  object has infinite extent. **/
  class SimTK_SIMMATH_EXPORT ContactGeometry::HalfSpace : public ContactGeometry {
  public:
  /** Create a %HalfSpace for contact, with surface passing through the origin
  and outward normal -XAxis (in its own frame). Thus the half-space is all 
  of x>0. When this is placed on a Body, a Transform is provided that 
  repositions the half-space to the desired location and orientation in the 
  Body's frame. **/
  HalfSpace();
  
  /** Return the %HalfSpace outward normal in its own frame as a unit vector. **/
  UnitVec3 getNormal() const;
  
  /** Return true if the supplied ContactGeometry object is a halfspace. **/
  static bool isInstance(const ContactGeometry& geo)
  {   return geo.getTypeId()==classTypeId(); }
  /** Cast the supplied ContactGeometry object to a const halfspace. **/
  static const HalfSpace& getAs(const ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<const HalfSpace&>(geo); }
  /** Cast the supplied ContactGeometry object to a writable halfspace. **/
  static HalfSpace& updAs(ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<HalfSpace&>(geo); }
  
  /** Obtain the unique id for HalfSpace contact geometry. **/
  static ContactGeometryTypeId classTypeId();
  
  class Impl; /**< Internal use only. **/
  const Impl& getImpl() const; /**< Internal use only. **/
  Impl& updImpl(); /**< Internal use only. **/
  };
  
  
  
  //==============================================================================
  //                                CYLINDER
  //==============================================================================
  /** This ContactGeometry subclass represents a cylinder centered at the
  origin, with radius r in the x-y plane, and infinite length along z.
  TODO: should allow finite length to be specified. **/
  class SimTK_SIMMATH_EXPORT ContactGeometry::Cylinder : public ContactGeometry {
  public:
  explicit Cylinder(Real radius);
  Real getRadius() const;
  void setRadius(Real radius);
  
  /** Return true if the supplied ContactGeometry object is a cylinder. **/
  static bool isInstance(const ContactGeometry& geo)
  {   return geo.getTypeId()==classTypeId(); }
  /** Cast the supplied ContactGeometry object to a const cylinder. **/
  static const Cylinder& getAs(const ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<const Cylinder&>(geo); }
  /** Cast the supplied ContactGeometry object to a writable cylinder. **/
  static Cylinder& updAs(ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<Cylinder&>(geo); }
  
  /** Obtain the unique id for Cylinder contact geometry. **/
  static ContactGeometryTypeId classTypeId();
  
  class Impl; /**< Internal use only. **/
  const Impl& getImpl() const; /**< Internal use only. **/
  Impl& updImpl(); /**< Internal use only. **/
  };
  
  
  
  //==============================================================================
  //                                  SPHERE
  //==============================================================================
  /** This ContactGeometry subclass represents a sphere centered at the 
  origin. **/
  class SimTK_SIMMATH_EXPORT ContactGeometry::Sphere : public ContactGeometry {
  public:
  explicit Sphere(Real radius);
  Real getRadius() const;
  void setRadius(Real radius);
  
  /** Return true if the supplied ContactGeometry object is a sphere. **/
  static bool isInstance(const ContactGeometry& geo)
  {   return geo.getTypeId()==classTypeId(); }
  /** Cast the supplied ContactGeometry object to a const sphere. **/
  static const Sphere& getAs(const ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<const Sphere&>(geo); }
  /** Cast the supplied ContactGeometry object to a writable sphere. **/
  static Sphere& updAs(ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<Sphere&>(geo); }
  
  /** Obtain the unique id for Sphere contact geometry. **/
  static ContactGeometryTypeId classTypeId();
  
  class Impl; /**< Internal use only. **/
  const Impl& getImpl() const; /**< Internal use only. **/
  Impl& updImpl(); /**< Internal use only. **/
  };
  
  
  
  //==============================================================================
  //                                  ELLIPSOID
  //==============================================================================
  /** This ContactGeometry subclass represents an ellipsoid centered at the 
  origin, with its principal axes pointing along the x, y, and z axes and
  half dimensions a,b, and c (all > 0) along those axes, respectively. The
  implicit equation f(x,y,z)=0 of the ellipsoid surface is <pre>
      f(x,y,z) = Ax^2+By^2+Cz^2 - 1
      where A=1/a^2, B=1/b^2, C=1/c^2     
  </pre>
  A,B, and C are the squares of the principal curvatures ka=1/a, kb=1/b, and
  kc=1/c.
  
  The interior of the ellipsoid consists of all points such that f(x,y,z)<0 and
  points exterior satisfy f(x,y,z)>0. The region around any point (x,y,z) on an 
  ellipsoid surface is locally an elliptic paraboloid with equation <pre>
      -2 z' = kmax x'^2 + kmin y'^2   
  </pre>
  where z' is measured along the the outward unit normal n at (x,y,z), x' is
  measured along the the unit direction u of maximum curvature, and y' is
  measured along the unit direction v of minimum curvature. kmax,kmin are the 
  curvatures with kmax >= kmin > 0. The signs of the mutually perpendicular
  vectors u and v are chosen so that (u,v,n) forms a right-handed coordinate 
  system for the paraboloid. **/
  class SimTK_SIMMATH_EXPORT ContactGeometry::Ellipsoid : public ContactGeometry {
  public:
  /** Construct an Ellipsoid given its three principal half-axis dimensions a,b,c
  (all positive) along the local x,y,z directions respectively. The curvatures 
  (reciprocals of radii) are precalculated here at a cost of about 30 flops. **/
  explicit Ellipsoid(const Vec3& radii);
  /** Obtain the three half-axis dimensions a,b,c used to define this
  ellipsoid. **/
  const Vec3& getRadii() const;
  /** Set the three half-axis dimensions a,b,c (all positive) used to define this
  ellipsoid, overriding the current radii and recalculating the principal 
  curvatures at a cost of about 30 flops. 
  @param[in] radii    The three half-dimensions of the ellipsoid, in the 
                      ellipsoid's local x, y, and z directions respectively. **/
  void setRadii(const Vec3& radii);
  
  /** For efficiency we precalculate the principal curvatures whenever the 
  ellipsoid radii are set; this avoids having to repeatedly perform these three
  expensive divisions at runtime. The curvatures are ka=1/a, kb=1/b, and kc=1/c 
  so that the ellipsoid's implicit equation can be written Ax^2+By^2+Cz^2=1, 
  with A=ka^2, etc. **/
  const Vec3& getCurvatures() const;
  
  /** Given a point \a P =(x,y,z) on the ellipsoid surface, return the unique unit
  outward normal to the ellipsoid at that point. If \a P is not on the surface, 
  the result is the same as for the point obtained by scaling the vector 
  \a P - O until it just touches the surface. That is, we compute 
  P'=findPointInThisDirection(P) and then return the normal at P'. Cost is about
  40 flops regardless of whether P was initially on the surface. 
  @param[in] P    A point on the ellipsoid surface, measured and expressed in the
                  ellipsoid's local frame. See text for what happens if \a P is
                  not actually on the ellipsoid surface.
  @return The outward-facing unit normal at point \a P (or at the surface point
  pointed to by \a P).
  @see findPointInSameDirection() **/
  UnitVec3 findUnitNormalAtPoint(const Vec3& P) const;
  
  /** Given a unit direction \a n, find the unique point P on the ellipsoid 
  surface at which the outward-facing normal is \a n. Cost is about 40 flops. 
  @param[in] n    The unit vector for which we want to find a match on the
                  ellipsoid surface, expressed in the ellipsoid's local frame. 
  @return The point on the ellipsoid's surface at which the outward-facing
  normal is the same as \a n. The point is measured and expressed in the 
  ellipsoid's local frame. **/
  Vec3 findPointWithThisUnitNormal(const UnitVec3& n) const;
  
  /** Given a direction d defined by the vector Q-O for an arbitrary point in 
  space Q=(x,y,z)!=O, find the unique point P on the ellipsoid surface that is 
  in direction d from the ellipsoid origin O. That is, P=s*d for some scalar 
  s > 0 such that f(P)=0. Cost is about 40 flops. 
  @param[in] Q    A point in space measured from the ellipsoid origin but not the
                  origin.
  @return P, the intersection of the ray in the direction Q-O with the ellipsoid
  surface **/
  Vec3 findPointInSameDirection(const Vec3& Q) const;
  
  /** Given a point Q on the surface of the ellipsoid, find the approximating
  paraboloid at Q in a frame P where OP=Q, Pz is the outward-facing unit
  normal to the ellipsoid at Q, Px is the direction of maximum curvature
  and Py is the direction of minimum curvature. k=(kmax,kmin) are the returned
  curvatures with kmax >= kmin > 0. The equation of the resulting paraboloid 
  in the P frame is -2z = kmax*x^2 + kmin*y^2. Cost is about 260 flops; you can
  save a little time if you already know the normal at Q by using the other
  overloaded signature for this method.
   
  @warning It is up to you to make sure that Q is actually on the ellipsoid
  surface. If it is not you will quietly get a meaningless result.
  
  @param[in]  Q       A point on the surface of this ellipsoid, measured and
                      expressed in the ellipsoid's local frame.
  @param[out] X_EP    The frame of the paraboloid P, measured and expressed in
                      the ellipsoid local frame E. X_EP.p() is \a Q, X_EP.x() 
                      is the calculated direction of maximum curvature kmax; y() 
                      is the direction of minimum curvature kmin; z is the 
                      outward facing normal at \a Q.
  @param[out] k       The maximum (k[0]) and minimum (k[1]) curvatures of the
                      ellipsoid (and paraboloid P) at point \a Q.
  @see findParaboloidAtPointWithNormal() **/
  void findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const;
  
  /** If you already have both a point and the unit normal at that point, this 
  will save about 40 flops by trusting that you have provided the correct normal;
  be careful -- no one is going to check that you got this right. The results are
  meaningless if the point and normal are not consistent. Cost is about 220 flops.
  @see findParaboloidAtPoint() for details **/
  void findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& n,
      Transform& X_EP, Vec2& k) const;
  
  /** Return true if the supplied ContactGeometry object is an Ellipsoid. **/
  static bool isInstance(const ContactGeometry& geo)
  {   return geo.getTypeId()==classTypeId(); }
  /** Cast the supplied ContactGeometry object to a const Ellipsoid. **/
  static const Ellipsoid& getAs(const ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<const Ellipsoid&>(geo); }
  /** Cast the supplied ContactGeometry object to a writable Ellipsoid. **/
  static Ellipsoid& updAs(ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<Ellipsoid&>(geo); }
  
  /** Obtain the unique id for Ellipsoid contact geometry. **/
  static ContactGeometryTypeId classTypeId();
  
  class Impl; /**< Internal use only. **/
  const Impl& getImpl() const; /**< Internal use only. **/
  Impl& updImpl(); /**< Internal use only. **/
  };
  
  
  
  //==============================================================================
  //                            SMOOTH HEIGHT MAP
  //==============================================================================
  /** This ContactGeometry subclass represents a smooth surface fit through a
  set of sampled points using bicubic patches to provide C2 continuity. It is
  particularly useful as a bounded terrain. The boundary is an axis-aligned
  rectangle in the local x-y plane. Within the boundary, every (x,y) location has
  a unique height z, so caves and overhangs cannot be represented.
  
  The surface is parameterized as z=f(x,y) where x,y,z are measured in 
  the surface's local coordinate frame. This can also be described as the 
  implicit function F(x,y,z)=f(x,y)-z=0, as though this were an infinitely thick
  slab in the -z direction below the surface. **/
  class SimTK_SIMMATH_EXPORT 
  ContactGeometry::SmoothHeightMap : public ContactGeometry {
  public:
  /** Create a SmoothHeightMap surface using an already-existing BicubicSurface.
  The BicubicSurface object is referenced, not copied, so it may be shared with
  other users. **/
  explicit SmoothHeightMap(const BicubicSurface& surface);
  
  /** Return a reference to the BicubicSurface object being used by this
  SmoothHeightMap. **/
  const BicubicSurface& getBicubicSurface() const;
  
  /** (Advanced) Return a reference to the oriented bounding box tree for this
  surface. **/
  const OBBTree& getOBBTree() const;
  
  /** Return true if the supplied ContactGeometry object is a SmoothHeightMap. **/
  static bool isInstance(const ContactGeometry& geo)
  {   return geo.getTypeId()==classTypeId(); }
  /** Cast the supplied ContactGeometry object to a const SmoothHeightMap. **/
  static const SmoothHeightMap& getAs(const ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<const SmoothHeightMap&>(geo); }
  /** Cast the supplied ContactGeometry object to a writable SmoothHeightMap. **/
  static SmoothHeightMap& updAs(ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<SmoothHeightMap&>(geo); }
  
  /** Obtain the unique id for SmoothHeightMap contact geometry. **/
  static ContactGeometryTypeId classTypeId();
  
  class Impl; /**< Internal use only. **/
  const Impl& getImpl() const; /**< Internal use only. **/
  Impl& updImpl(); /**< Internal use only. **/
  };
  
  
  //==============================================================================
  //                                  BRICK
  //==============================================================================
  /** This ContactGeometry subclass represents a rectangular solid centered at the
  origin. This object is finite, convex, and non-smooth. **/
  class SimTK_SIMMATH_EXPORT ContactGeometry::Brick : public ContactGeometry {
  public:
  /** Create a brick-shaped contact shape of the given half-dimensions, expressed
  in the brick's local frame. **/
  explicit Brick(const Vec3& halfLengths);
  /** Get the half-dimensions of this %Brick, expressed in its own frame. These
  are also the coordinates of the vertex in the +,+,+ octant, measured from the
  %Brick-frame origin which is its center point. **/
  const Vec3& getHalfLengths() const;
  /** Change the shape or size of this brick by setting its half-dimensions. **/
  void setHalfLengths(const Vec3& halfLengths);
  
  /** Get the Geo::Box object used to represent this %Brick. **/
  const Geo::Box& getGeoBox() const;
  
  /** Return true if the supplied ContactGeometry object is a brick. **/
  static bool isInstance(const ContactGeometry& geo)
  {   return geo.getTypeId()==classTypeId(); }
  /** Cast the supplied ContactGeometry object to a const brick. **/
  static const Brick& getAs(const ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<const Brick&>(geo); }
  /** Cast the supplied ContactGeometry object to a writable brick. **/
  static Brick& updAs(ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<Brick&>(geo); }
  
  /** Obtain the unique id for Brick contact geometry. **/
  static ContactGeometryTypeId classTypeId();
  
  class Impl; /**< Internal use only. **/
  const Impl& getImpl() const; /**< Internal use only. **/
  Impl& updImpl(); /**< Internal use only. **/
  };
  
  
  //==============================================================================
  //                              TRIANGLE MESH
  //==============================================================================
  /** This ContactGeometry subclass represents an arbitrary shape described by a 
  mesh of triangular faces. The mesh surface must satisfy the following 
  requirements:
    - It must be closed, so that any point can unambiguously be classified as 
      either inside or outside.
    - It may not intersect itself anywhere, even at a single point.
    - It must be an oriented manifold.
    - The vertices for each face must be ordered counter-clockwise when viewed
      from the outside. That is, if v0, v1, and v2 are the locations of the 
      three vertices for a face, the cross product (v1-v0)%(v2-v0) must point 
      outward.
    - The length of every edge must be non-zero.
  
  It is your responsibility to ensure that any mesh you create meets these 
  requirements. The constructor will detect many incorrect meshes and signal them
  by throwing an exception, but it is not guaranteed to detect all possible 
  problems.  If a mesh fails to satisfy any of these requirements, the results of
  calculations performed with it are undefined. For example, collisions involving
  it might fail to be detected, or contact forces on it might be calculated 
  incorrectly. **/
  class SimTK_SIMMATH_EXPORT ContactGeometry::TriangleMesh 
  :   public ContactGeometry {
  public:
  class OBBTreeNode;
  /** Create a TriangleMesh.
  @param vertices     The positions of all vertices in the mesh.
  @param faceIndices  The indices of the vertices that make up each face. The 
                      first three elements are the vertices in the first face, 
                      the next three elements are the vertices in the second 
                      face, etc.
  @param smooth       If true, the mesh will be treated as a smooth surface, and 
                      normal vectors will be smoothly interpolated between 
                      vertices. If false, it will be treated as a faceted mesh 
                      with a constant normal vector over each face. **/
  TriangleMesh(const ArrayViewConst_<Vec3>& vertices, const ArrayViewConst_<int>& faceIndices, bool smooth=false);
  /** Create a TriangleMesh based on a PolygonalMesh object. If any faces of the 
  PolygonalMesh have more than three vertices, they are automatically 
  triangulated.
  @param mesh      The PolygonalMesh from which to construct a triangle mesh.
  @param smooth    If true, the mesh will be treated as a smooth surface, and 
                   normal vectors will be smoothly interpolated between vertices.
                   If false, it will be treated as a faceted mesh with a constant
                   normal vector over each face. **/
  explicit TriangleMesh(const PolygonalMesh& mesh, bool smooth=false);
  /** Get the number of edges in the mesh. **/
  int getNumEdges() const;
  /** Get the number of faces in the mesh. **/
  int getNumFaces() const;
  /** Get the number of vertices in the mesh. **/
  int getNumVertices() const;
  /** Get the position of a vertex in the mesh.
  @param index  The index of the vertex to get.
  @return The position of the specified vertex. **/
  const Vec3& getVertexPosition(int index) const;
  /** Get the index of one of the edges of a face. Edge 0 connects vertices 0 
  and 1. Edge 1 connects vertices 1 and 2. Edge 2 connects vertices 0 and 2.
  @param face    The index of the face.
  @param edge    The index of the edge within the face (0, 1, or 2).
  @return The index of the specified edge. **/
  int getFaceEdge(int face, int edge) const;
  /** Get the index of one of the vertices of a face.
  @param face    The index of the face.
  @param vertex  The index of the vertex within the face (0, 1, or 2).
  @return The index of the specified vertex. **/
  int getFaceVertex(int face, int vertex) const;
  /** Get the index of one of the faces shared by an edge
  @param edge    The index of the edge.
  @param face    The index of the face within the edge (0 or 1).
  @return The index of the specified face. **/
  int getEdgeFace(int edge, int face) const;
  /** Get the index of one of the vertices shared by an edge.
  @param edge    The index of the edge.
  @param vertex  The index of the vertex within the edge (0 or 1).
  @return The index of the specified vertex. **/
  int getEdgeVertex(int edge, int vertex) const;
  /** Find all edges that intersect a vertex.
  @param vertex  The index of the vertex.
  @param edges   The indices of all edges intersecting the vertex will be added
                 to this. **/
  void findVertexEdges(int vertex, Array_<int>& edges) const;
  /** Get the normal vector for a face. This points outward from the mesh.
  @param face    The index of the face. **/
  const UnitVec3& getFaceNormal(int face) const;
  /** Get the area of a face.
  @param face    The index of the face. **/
  Real getFaceArea(int face) const;
  /** Calculate the location of a point on the surface, in the local frame of
  the TriangleMesh. Cost is 11 flops. 
  @param face    The index of the face containing the point.
  @param uv      The point within the face, specified by its barycentric uv 
                 coordinates. **/
  Vec3 findPoint(int face, const Vec2& uv) const;
  /** Calculate the location of a face's centroid, that is, the point uv=(1/3,1/3)
  which is the average of the three vertex locations. This is a common special 
  case of findPoint() that can be calculated more quickly (7 flops).
  @param face    The index of the face whose centroid is of interest. **/
  Vec3 findCentroid(int face) const;
  /** Calculate the normal vector at a point on the surface.
  @param face    The index of the face containing the point.
  @param uv      The point within the face, specified by its barycentric uv 
                 coordinates. **/
  UnitVec3 findNormalAtPoint(int face, const Vec2& uv) const;
  /** Given a point, find the nearest point on the surface of this object. If 
  multiple points on the surface are equally close to the specified point, this 
  may return any of them.
  @param position    The point in question.
  @param inside      On exit, this is set to true if the specified point is 
                     inside this object, false otherwise.
  @param normal      On exit, this contains the surface normal at the returned 
                     point.
  @return The point on the surface of the object which is closest to the 
  specified point. **/
  Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
  /** Given a point, find the nearest point on the surface of this object. If 
  multiple points on the surface are equally close to the specified point, this 
  may return any of them.
  @param position    The point in question.
  @param inside      On exit, this is set to true if the specified point is 
                     inside this object, false otherwise.
  @param face        On exit, this contains the index of the face containing the 
                     returned point.
  @param uv          On exit, this contains the barycentric coordinates (u and v)
                     of the returned point within its face.
  @return The point on the surface of the object which is closest to the 
  specified point. **/
  Vec3 findNearestPoint(const Vec3& position, bool& inside, int& face, Vec2& uv) const;
  
  /** Given a point and a face of this object, find the point of the face that is
  nearest the given point. If multiple points on the face are equally close to 
  the specified point, this may return any of them.
  @param position    The point in question.
  @param face        The face to be examined.
  @param uv          On exit, this contains the barycentric coordinates (u and v)
                     of the returned point within the face.
  @return The face point, in the surface's frame, that is closest to the 
  specified point. **/
  Vec3 findNearestPointToFace(const Vec3& position, int face, Vec2& uv) const;
  
  
  /** Determine whether this mesh intersects a ray, and if so, find the 
  intersection point.
  @param origin     The position at which the ray begins.
  @param direction  The ray direction.
  @param distance   If an intersection is found, the distance from the ray origin
                    to the intersection point is stored in this. Otherwise, it is
                    left unchanged.
  @param normal     If an intersection is found, the surface normal of the 
                    intersection point is stored in this. Otherwise, it is left 
                    unchanged.
  @return \c true if an intersection is found, \c false otherwise. **/
  bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const;
  /** Determine whether this mesh intersects a ray, and if so, find what face it 
  hit.
  @param origin     The position at which the ray begins.
  @param direction  The ray direction.
  @param distance   If an intersection is found, the distance from the ray origin
                    to the intersection point is stored in this. Otherwise, it is
                    left unchanged.
  @param face       If an intersection is found, the index of the face hit by the
                    ray is stored in this. Otherwise, it is left unchanged.
  @param uv         If an intersection is found, the barycentric coordinates (u 
                    and v) of the intersection point within the hit face are 
                    stored in this. Otherwise, it is left unchanged.
  @return \c true if an intersection is found, \c false otherwise. **/
  bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, int& face, Vec2& uv) const;
  /** Get the OBBTreeNode which forms the root of this mesh's Oriented Bounding 
  Box Tree. **/
  OBBTreeNode getOBBTreeNode() const;
  
  /** Generate a PolygonalMesh from this TriangleMesh; useful mostly for debugging
  because you can create a DecorativeMesh from this and then look at it. **/
  PolygonalMesh createPolygonalMesh() const;
  
  /** Return true if the supplied ContactGeometry object is a triangle mesh. **/
  static bool isInstance(const ContactGeometry& geo)
  {   return geo.getTypeId()==classTypeId(); }
  /** Cast the supplied ContactGeometry object to a const triangle mesh. **/
  static const TriangleMesh& getAs(const ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<const TriangleMesh&>(geo); }
  /** Cast the supplied ContactGeometry object to a writable triangle mesh. **/
  static TriangleMesh& updAs(ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<TriangleMesh&>(geo); }
  
  /** Obtain the unique id for TriangleMesh contact geometry. **/
  static ContactGeometryTypeId classTypeId();
  
  class Impl; /**< Internal use only. **/
  const Impl& getImpl() const; /**< Internal use only. **/
  Impl& updImpl(); /**< Internal use only. **/
  };
  
  
  
  //==============================================================================
  //                       TRIANGLE MESH :: OBB TREE NODE
  //==============================================================================
  /** This class represents a node in the Oriented Bounding Box Tree for a 
  TriangleMesh. Each node has an OrientedBoundingBox that fully encloses all 
  triangles contained within it or its  children. This is a binary tree: each 
  non-leaf node has two children. Triangles are stored only in the leaf nodes. **/
  class SimTK_SIMMATH_EXPORT ContactGeometry::TriangleMesh::OBBTreeNode {
  public:
  OBBTreeNode(const OBBTreeNodeImpl& impl);
  /** Get the OrientedBoundingBox which encloses all triangles in this node or 
  its children. **/
  const OrientedBoundingBox& getBounds() const;
  /** Get whether this is a leaf node. **/
  bool isLeafNode() const;
  /** Get the first child node. Calling this on a leaf node will produce an 
  exception. **/
  const OBBTreeNode getFirstChildNode() const;
  /** Get the second child node. Calling this on a leaf node will produce an 
  exception. **/
  const OBBTreeNode getSecondChildNode() const;
  /** Get the indices of all triangles contained in this node. Calling this on a
  non-leaf node will produce an exception. **/
  const Array_<int>& getTriangles() const;
  /** Get the number of triangles inside this node. If this is not a leaf node,
  this is the total number of triangles contained by all children of this
  node. **/
  int getNumTriangles() const;
  
  private:
  const OBBTreeNodeImpl* impl;
  };
  
  //==============================================================================
  //                                TORUS
  //==============================================================================
  /** This ContactGeometry subclass represents a torus centered at the
  origin with the axial direction aligned to the z-axis. It is defined by
  a torusRadius (radius of the circular centerline of the torus, measured
  from the origin), and a tubeRadius (radius of the torus cross-section:
  perpendicular distance from the circular centerline to the surface). **/
  class SimTK_SIMMATH_EXPORT ContactGeometry::Torus : public ContactGeometry {
  public:
  Torus(Real torusRadius, Real tubeRadius);
  Real getTorusRadius() const;
  void setTorusRadius(Real radius);
  Real getTubeRadius() const;
  void setTubeRadius(Real radius);
  
  /** Return true if the supplied ContactGeometry object is a torus. **/
  static bool isInstance(const ContactGeometry& geo)
  {   return geo.getTypeId()==classTypeId(); }
  /** Cast the supplied ContactGeometry object to a const torus. **/
  static const Torus& getAs(const ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<const Torus&>(geo); }
  /** Cast the supplied ContactGeometry object to a writable torus. **/
  static Torus& updAs(ContactGeometry& geo)
  {   assert(isInstance(geo)); return static_cast<Torus&>(geo); }
  
  /** Obtain the unique id for Torus contact geometry. **/
  static ContactGeometryTypeId classTypeId();
  
  class Impl; /**< Internal use only. **/
  const Impl& getImpl() const; /**< Internal use only. **/
  Impl& updImpl(); /**< Internal use only. **/
  };
  
  
  
  
  //==============================================================================
  //                     GEODESIC EVALUATOR helper classes
  //==============================================================================
  
  
  /**
   * A simple plane class
   **/
  class Plane {
  public:
      Plane() : m_normal(1,0,0), m_offset(0) { }
      Plane(const Vec3& normal, const Real& offset)
      :   m_normal(normal), m_offset(offset) { }
  
      Real getDistance(const Vec3& pt) const {
          return ~m_normal*pt - m_offset;
      }
  
      Vec3 getNormal() const {
          return m_normal;
      }
  
      Real getOffset() const {
          return m_offset;
      }
  
  private:
      Vec3 m_normal;
      Real m_offset;
  }; // class Plane
  
  
  /**
   * A event handler to terminate integration when geodesic hits the plane.
   * For use with a ParticleConSurfaceSystem
   **/
  class GeodHitPlaneEvent : public TriggeredEventHandler {
  public:
      GeodHitPlaneEvent()
      :   TriggeredEventHandler(Stage::Position) { }
  
      explicit GeodHitPlaneEvent(const Plane& aplane)
      :   TriggeredEventHandler(Stage::Position) {
          plane = aplane;
      }
  
      // event is triggered if distance of geodesic endpoint to plane is zero
      Real getValue(const State& state) const override {
          if (!enabled) {
              return 1;
          }
          Vec3 endpt(&state.getQ()[0]);
          Real dist =  plane.getDistance(endpt);
  //        std::cout << "dist = " << dist << std::endl;
          return dist;
      }
  
      // This method is called whenever this event occurs.
      void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const override {
          if (!enabled) {
              return;
          }
  
          // This should be triggered when geodesic endpoint to plane is zero.
          Vec3 endpt;
          const Vector& q = state.getQ();
          endpt[0] = q[0]; endpt[1] = q[1]; endpt[2] = q[2];
          Real dist = plane.getDistance(endpt);
  
  //        ASSERT(std::abs(dist) < 0.01 );
          shouldTerminate = true;
  //        std::cout << "hit plane!" << std::endl;
      }
  
      void setPlane(const Plane& aplane) const {
          plane = aplane;
      }
  
      const Plane& getPlane() const {
          return plane;
      }
  
      const void setEnabled(bool enabledFlag) {
          enabled = enabledFlag;
      }
  
      const bool isEnabled() {
          return enabled;
      }
  
  private:
      mutable Plane plane;
      bool enabled;
  
  }; // class GeodHitPlaneEvent
  
  /**
   * This class generates decoration for contact points and straight line path segments
   **/
  class PathDecorator : public DecorationGenerator {
  public:
      PathDecorator(const Vector& x, const Vec3& O, const Vec3& I, const Vec3& color) :
              m_x(x), m_O(O), m_I(I), m_color(color) { }
  
      virtual void generateDecorations(const State& state,
              Array_<DecorativeGeometry>& geometry) override {
  //        m_system.realize(state, Stage::Position);
  
          Vec3 P, Q;
          P[0] = m_x[0]; P[1] = m_x[1]; P[2] = m_x[2];
          Q[0] = m_x[3]; Q[1] = m_x[4]; Q[2] = m_x[5];
  
          geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_O));
          geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(P));
          geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(Q));
          geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_I));
  
          geometry.push_back(DecorativeLine(m_O,P)
                  .setColor(m_color)
                  .setLineThickness(2));
          geometry.push_back(DecorativeLine(Q,m_I)
                  .setColor(m_color)
                  .setLineThickness(2));
  
      }
  
  private:
      const Vector& m_x; // x = ~[P Q]
      const Vec3& m_O;
      const Vec3& m_I;
      const Vec3& m_color;
      Rotation R_plane;
      Vec3 offset;
  }; // class DecorationGenerator
  
  
  /**
   * This class generates decoration for a plane
   **/
  class PlaneDecorator : public DecorationGenerator {
  public:
      PlaneDecorator(const Plane& plane, const Vec3& color) :
              m_plane(plane), m_color(color) { }
  
      virtual void generateDecorations(const State& state,
              Array_<DecorativeGeometry>& geometry) override {
  //        m_system.realize(state, Stage::Position);
  
          // draw plane
          R_plane.setRotationFromOneAxis(UnitVec3(m_plane.getNormal()),
                  CoordinateAxis::XCoordinateAxis());
          offset = 0;
          offset[0] = m_plane.getOffset();
          geometry.push_back(
                  DecorativeBrick(Vec3(Real(.01),1,1))
                  .setTransform(Transform(R_plane, R_plane*offset))
                  .setColor(m_color)
                  .setOpacity(Real(.2)));
      }
  
  private:
      const Plane& m_plane;
      const Vec3& m_color;
      Rotation R_plane;
      Vec3 offset;
  }; // class DecorationGenerator
  
  
  } // namespace SimTK
  
  #endif // SimTK_SIMMATH_CONTACT_GEOMETRY_H_
  