Simbody
3.7
|
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. More...
Classes | |
class | Brick |
This ContactGeometry subclass represents a rectangular solid centered at the origin. More... | |
class | Cylinder |
This ContactGeometry subclass represents a cylinder centered at the origin, with radius r in the x-y plane, and infinite length along z. More... | |
class | 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. More... | |
class | HalfSpace |
This ContactGeometry subclass represents an object that occupies the entire half-space x>0. More... | |
class | SmoothHeightMap |
This ContactGeometry subclass represents a smooth surface fit through a set of sampled points using bicubic patches to provide C2 continuity. More... | |
class | Sphere |
This ContactGeometry subclass represents a sphere centered at the origin. More... | |
class | Torus |
This ContactGeometry subclass represents a torus centered at the origin with the axial direction aligned to the z-axis. More... | |
class | TriangleMesh |
This ContactGeometry subclass represents an arbitrary shape described by a mesh of triangular faces. More... | |
Public Member Functions | |
ContactGeometry () | |
Base class default constructor creates an empty handle. More... | |
ContactGeometry (const ContactGeometry &src) | |
Copy constructor makes a deep copy. More... | |
ContactGeometry & | operator= (const ContactGeometry &src) |
Copy assignment makes a deep copy. More... | |
~ContactGeometry () | |
Base class destructor deletes the implementation object. Note that this is not virtual; handles should consist of just a pointer to the implementation. More... | |
DecorativeGeometry | createDecorativeGeometry () const |
Generate a DecorativeGeometry that matches the shape of this ContactGeometry. More... | |
Vec3 | findNearestPoint (const Vec3 &position, bool &inside, UnitVec3 &normal) const |
Given a point, find the nearest point on the surface of this object. More... | |
Vec3 | projectDownhillToNearestPoint (const Vec3 &pointQ) const |
Given a query point Q, find the nearest point P on the surface of this object, looking only down the local gradient. More... | |
bool | trackSeparationFromLine (const Vec3 &pointOnLine, const UnitVec3 &directionOfLine, const Vec3 &startingGuessForClosestPoint, Vec3 &newClosestPointOnSurface, Vec3 &closestPointOnLine, Real &height) 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. More... | |
bool | intersectsRay (const Vec3 &origin, const UnitVec3 &direction, Real &distance, UnitVec3 &normal) const |
Determine whether this object intersects a ray, and if so, find the intersection point. More... | |
void | getBoundingSphere (Vec3 ¢er, Real &radius) const |
Get a bounding sphere which completely encloses this object. More... | |
bool | isSmooth () const |
Returns true if this is a smooth surface, meaning that it can provide meaningful curvature information and continuous derivatives with respect to its parameterization. More... | |
void | calcCurvature (const Vec3 &point, Vec2 &curvature, Rotation &orientation) const |
Compute the principal curvatures and their directions, and the surface normal, at a given point on a smooth surface. More... | |
const Function & | getImplicitFunction () const |
Our smooth surfaces define a function f(P)=0 that provides an implicit representation of the surface. More... | |
Real | calcSurfaceValue (const Vec3 &point) const |
Calculate the value of the implicit surface function, at a given point. More... | |
UnitVec3 | calcSurfaceUnitNormal (const Vec3 &point) const |
Calculate the implicit surface outward facing unit normal at the given point. More... | |
Vec3 | calcSurfaceGradient (const Vec3 &point) const |
Calculate the gradient of the implicit surface function, at a given point. More... | |
Mat33 | calcSurfaceHessian (const Vec3 &point) const |
Calculate the hessian of the implicit surface function, at a given point. More... | |
Real | calcGaussianCurvature (const Vec3 &gradient, const Mat33 &Hessian) 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. More... | |
Real | calcGaussianCurvature (const Vec3 &point) 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. More... | |
Real | calcSurfaceCurvatureInDirection (const Vec3 &point, const UnitVec3 &direction) const |
For an implicit surface, return the curvature k of the surface at a given point p in a given direction tp. More... | |
void | calcSurfacePrincipalCurvatures (const Vec3 &point, Vec2 &curvature, Rotation &R_SP) 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. More... | |
bool | isConvex () const |
Returns true if this surface is known to be convex. More... | |
Vec3 | calcSupportPoint (UnitVec3 direction) 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). More... | |
ContactGeometryTypeId | getTypeId () const |
ContactTrackerSubsystem uses this id for fast identification of specific surface shapes. More... | |
ContactGeometry (ContactGeometryImpl *impl) | |
Internal use only. More... | |
bool | isOwnerHandle () const |
Internal use only. More... | |
bool | isEmptyHandle () const |
Internal use only. More... | |
bool | hasImpl () const |
Internal use only. More... | |
const ContactGeometryImpl & | getImpl () const |
Internal use only. More... | |
ContactGeometryImpl & | updImpl () |
Internal use only. More... | |
Geodesic Evaluators | |
void | initGeodesic (const Vec3 &xP, const Vec3 &xQ, const Vec3 &xSP, const GeodesicOptions &options, Geodesic &geod) const |
Given two points, find a geodesic curve connecting them. More... | |
void | continueGeodesic (const Vec3 &xP, const Vec3 &xQ, const Geodesic &prevGeod, 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. More... | |
void | makeStraightLineGeodesic (const Vec3 &xP, const Vec3 &xQ, const UnitVec3 &defaultDirectionIfNeeded, const GeodesicOptions &options, Geodesic &geod) const |
Produce a straight-line approximation to the (presumably short) geodesic between two points on this implicit surface. More... | |
void | shootGeodesicInDirectionUntilLengthReached (const Vec3 &xP, const UnitVec3 &tP, const Real &terminatingLength, 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. More... | |
void | calcGeodesicReverseSensitivity (Geodesic &geodesic, const Vec2 &initSensitivity=Vec2(0, 1)) 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. More... | |
void | shootGeodesicInDirectionUntilPlaneHit (const Vec3 &xP, const UnitVec3 &tP, const Plane &terminatingPlane, const GeodesicOptions &options, Geodesic &geod) const |
Compute a geodesic curve starting at the given point, starting in the given direction, and terminating when it hits the given plane. More... | |
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 split geodesic method with initial shooting directions tPhint and -tQhint. More... | |
void | calcGeodesicUsingOrthogonalMethod (const Vec3 &xP, const Vec3 &xQ, const Vec3 &tPhint, Real lengthHint, Geodesic &geod) const |
Utility method to find geodesic between P and Q using the orthogonal method, with initial direction tPhint and initial length lengthHint. More... | |
void | calcGeodesicUsingOrthogonalMethod (const Vec3 &xP, const Vec3 &xQ, Geodesic &geod) const |
This signature makes a guess at the initial direction and length and then calls the other signature. More... | |
Vec2 | calcSplitGeodError (const Vec3 &P, const Vec3 &Q, const UnitVec3 &tP, const UnitVec3 &tQ, Geodesic *geod=0) const |
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. More... | |
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 at the given length. More... | |
void | shootGeodesicInDirectionUntilPlaneHitAnalytical (const Vec3 &xP, const UnitVec3 &tP, const Plane &terminatingPlane, 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. More... | |
void | calcGeodesicAnalytical (const Vec3 &xP, const Vec3 &xQ, const Vec3 &tPhint, const Vec3 &tQhint, Geodesic &geod) const |
Utility method to analytically find geodesic between P and Q with initial shooting directions tPhint and tQhint. More... | |
Vec2 | calcSplitGeodErrorAnalytical (const Vec3 &P, const Vec3 &Q, const UnitVec3 &tP, const UnitVec3 &tQ, Geodesic *geod=0) 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. More... | |
Geodesic-related Debugging | |
const Plane & | getPlane () const |
Get the plane associated with the geodesic hit plane event handler. More... | |
void | setPlane (const Plane &plane) const |
Set the plane associated with the geodesic hit plane event handler. More... | |
const Geodesic & | getGeodP () const |
Get the geodesic for access by visualizer. More... | |
const Geodesic & | getGeodQ () const |
Get the geodesic for access by visualizer. More... | |
const int | getNumGeodesicsShot () const |
Get the plane associated with the geodesic hit plane event handler. More... | |
void | addVizReporter (ScheduledEventReporter *reporter) const |
Get the plane associated with the geodesic hit plane event handler. More... | |
Static Public Member Functions | |
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) |
Calculate surface curvature at a point using differential geometry as suggested by Harris 2006, "Curvature of ellipsoids and other surfaces" Ophthal. More... | |
static void | combineParaboloids (const Rotation &R_SP1, const Vec2 &k1, const UnitVec3 &x2, const Vec2 &k2, Rotation &R_SP, Vec2 &k) |
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. More... | |
static void | combineParaboloids (const Rotation &R_SP1, const Vec2 &k1, const UnitVec3 &x2, const Vec2 &k2, 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. More... | |
Protected Attributes | |
ContactGeometryImpl * | impl |
Internal use only. More... | |
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:
Finite surfaces provide
Smooth surfaces provide
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.
|
inline |
Base class default constructor creates an empty handle.
SimTK::ContactGeometry::ContactGeometry | ( | const ContactGeometry & | src | ) |
Copy constructor makes a deep copy.
SimTK::ContactGeometry::~ContactGeometry | ( | ) |
Base class destructor deletes the implementation object. Note that this is not virtual; handles should consist of just a pointer to the implementation.
|
explicit |
Internal use only.
ContactGeometry& SimTK::ContactGeometry::operator= | ( | const ContactGeometry & | src | ) |
Copy assignment makes a deep copy.
DecorativeGeometry SimTK::ContactGeometry::createDecorativeGeometry | ( | ) | const |
Generate a DecorativeGeometry that matches the shape of this ContactGeometry.
Vec3 SimTK::ContactGeometry::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.
[in] | position | The point in question. |
[out] | inside | On exit, this is set to true if the specified point is inside this object, false otherwise. |
[out] | normal | On exit, this contains the surface normal at the returned point. |
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.
[in] | pointQ | The query point Q, assumed to be somewhere not too far from the surface. |
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.
bool SimTK::ContactGeometry::trackSeparationFromLine | ( | const Vec3 & | pointOnLine, |
const UnitVec3 & | directionOfLine, | ||
const Vec3 & | startingGuessForClosestPoint, | ||
Vec3 & | newClosestPointOnSurface, | ||
Vec3 & | closestPointOnLine, | ||
Real & | height | ||
) | 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, not one of the intersection points. The perpendicular point will be the point of 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 height argument.
[in] | pointOnLine | Any point through which the line passes. |
[in] | directionOfLine | A unit vector giving the direction of the line. |
[in] | startingGuessForClosestPoint | A point on the implicit surface that is a good guess for the closest (or most deeply penetrated) point. |
[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. |
[out] | closestPointOnLine | The is the corresponding point on the line that is the closest (furthest) from newClosestPointOnSurface. |
[out] | height | This is the signed height of the closest point on the line over the surface tangent plane at newClosestPointOnSurface. This is positive when 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. |
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 some points on the surface and the line that reduced the error function. Sometimes those are useful for visualizing what went wrong.
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:
To be precise we solve the following nonlinear system of three equations:
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.
bool SimTK::ContactGeometry::intersectsRay | ( | const Vec3 & | origin, |
const UnitVec3 & | direction, | ||
Real & | distance, | ||
UnitVec3 & | normal | ||
) | const |
Determine whether this object intersects a ray, and if so, find the intersection point.
[in] | origin | The position at which the ray begins. |
[in] | direction | The ray direction. |
[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. |
[out] | normal | If an intersection is found, the surface normal of the intersection point is stored in this. Otherwise, it is left unchanged. |
true
if an intersection is found, false
otherwise. Get a bounding sphere which completely encloses this object.
[out] | center | On exit, this contains the location of the center of the bounding sphere. |
[out] | radius | On exit, this contains the radius of the bounding sphere. |
bool SimTK::ContactGeometry::isSmooth | ( | ) | const |
Returns true
if this is a smooth surface, meaning that it can provide meaningful curvature information and continuous derivatives with respect to its parameterization.
void SimTK::ContactGeometry::calcCurvature | ( | const Vec3 & | point, |
Vec2 & | curvature, | ||
Rotation & | orientation | ||
) | const |
Compute the principal curvatures and their directions, and the surface normal, at a given point on a smooth surface.
[in] | point | A point at which to compute the curvature. |
[out] | curvature | On return, this will contain the maximum (curvature[0]) and minimum (curvature[1]) curvatures of the surface at the point. |
[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.
const Function& SimTK::ContactGeometry::getImplicitFunction | ( | ) | 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.
Calculate the value of the implicit surface function, at a given point.
[in] | point | A point at which to compute the surface value. |
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.
Calculate the gradient of the implicit surface function, at a given point.
[in] | point | A point at which to compute the surface gradient. |
Calculate the hessian of the implicit surface function, at a given point.
[in] | point | A point at which to compute the surface Hessian. |
Real SimTK::ContactGeometry::calcGaussianCurvature | ( | const Vec3 & | gradient, |
const Mat33 & | Hessian | ||
) | 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:
~grad(f) * Adjoint(H) * grad(f) Kg = -------------------------------- |grad(f)|^4
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:
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 ]
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 SimTK::ContactGeometry::calcSurfaceCurvatureInDirection | ( | const Vec3 & | point, |
const UnitVec3 & | direction | ||
) | const |
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
k = ~tp * H * tp / |g|,
where H is the Hessian matrix evaluated at p.
void SimTK::ContactGeometry::calcSurfacePrincipalCurvatures | ( | const Vec3 & | point, |
Vec2 & | curvature, | ||
Rotation & | R_SP | ||
) | 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.
bool SimTK::ContactGeometry::isConvex | ( | ) | const |
Returns true
if this surface is known to be convex.
This can be true for smooth or polygonal surfaces.
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.
ContactGeometryTypeId SimTK::ContactGeometry::getTypeId | ( | ) | const |
ContactTrackerSubsystem uses this id for fast identification of specific surface shapes.
|
static |
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
E = dxdu^2, F = ~dxdu * dxdv, G=dxdv^2
Second fundamental form: II = e du^2 + 2f dudv + g dv^2
e = - ~d2xdu2 * nn, f = - ~d2xdudv * nn, g = - ~d2xdv2 * nn
Given a direction t=dv/du, curvature k is
II e + 2f t + g t^2 k = -- = ---------------- (eq. 6-3) I E + 2F t + G t^2
We want minimum and maximum values for k to get principal curvatures. We can find those as the solutions to dk/dt=0.
dk/dt = (E + 2Ft + Gt^2)(f+gt) - (e + 2ft + gt^2)(F+Gt)
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:
A t^2 + B t + C = 0
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
curvatures: ~115 flops directions: ~50 flops ---- ~165 flops
|
static |
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:
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:
P1: -2z = kmax1 x1^2 + kmin1 y1^2 P2: 2z = kmax2 x2^2 + kmin2 y2^2
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
P: -2z = kmax x^2 + kmin y^2
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.
[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. |
[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. |
[in] | x2 | The direction of maximum curvature for paraboloid P2. x2 must be in the x1,y1 plane provided in R_SP1 and expressed in the S frame. |
[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. |
[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. |
[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. |
|
static |
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.
void SimTK::ContactGeometry::initGeodesic | ( | const Vec3 & | xP, |
const Vec3 & | xQ, | ||
const Vec3 & | xSP, | ||
const GeodesicOptions & | options, | ||
Geodesic & | geod | ||
) | const |
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.
[in] | xP | Coordinates of starting point P, in S. |
[in] | xQ | Coordinates of ending point Q, in S. |
[in] | xSP | (Optional) Coordinates of a preferred point for the geodesic to be near |
[in] | options | Parameters related to geodesic calculation |
[out] | geod | On exit, this contains a geodesic between P and Q. |
void SimTK::ContactGeometry::continueGeodesic | ( | const Vec3 & | xP, |
const Vec3 & | xQ, | ||
const Geodesic & | prevGeod, | ||
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.
[in] | xP | Coordinates of starting point P, in S. |
[in] | xQ | Coordinates of ending point Q, in S. |
[in] | prevGeod | The previous geodesic to which the new one should be similar. |
[in] | options | Parameters controlling the computation of the new geodesic. |
[out] | geod | On exit, this contains a geodesic between P and Q. |
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.
void SimTK::ContactGeometry::makeStraightLineGeodesic | ( | const Vec3 & | xP, |
const Vec3 & | xQ, | ||
const UnitVec3 & | defaultDirectionIfNeeded, | ||
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 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 numerically coincident, we will shift both of them to the midpoint (P'+Q')/2 so that the geodesic end points become 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 SimTK::ContactGeometry::shootGeodesicInDirectionUntilLengthReached | ( | const Vec3 & | xP, |
const UnitVec3 & | tP, | ||
const Real & | terminatingLength, | ||
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.
[in] | xP | Coordinates of the starting point for the geodesic. |
[in] | tP | The starting tangent direction for the geodesic. |
[in] | terminatingLength | The length that the resulting geodesic should have. |
[in] | options | Parameters related to geodesic calculation |
[out] | geod | On exit, this contains the calculated geodesic |
void SimTK::ContactGeometry::calcGeodesicReverseSensitivity | ( | Geodesic & | geodesic, |
const Vec2 & | initSensitivity = Vec2(0, 1) |
||
) | 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.
[in,out] | geodesic | An already-calculated geodesic. |
[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 SimTK::ContactGeometry::shootGeodesicInDirectionUntilPlaneHit | ( | const Vec3 & | xP, |
const UnitVec3 & | tP, | ||
const Plane & | terminatingPlane, | ||
const GeodesicOptions & | options, | ||
Geodesic & | geod | ||
) | const |
Compute a geodesic curve starting at the given point, starting in the given direction, and terminating when it hits the given plane.
[in] | xP | Coordinates of the starting point for the geodesic. |
[in] | tP | The starting tangent direction for the geodesic. |
[in] | terminatingPlane | The plane in which the end point of the resulting geodesic should lie. |
[in] | options | Parameters related to geodesic calculation |
[out] | geod | On exit, this contains the calculated geodesic |
void SimTK::ContactGeometry::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 split geodesic method with initial shooting directions tPhint and -tQhint.
void SimTK::ContactGeometry::calcGeodesicUsingOrthogonalMethod | ( | const Vec3 & | xP, |
const Vec3 & | xQ, | ||
const Vec3 & | tPhint, | ||
Real | lengthHint, | ||
Geodesic & | geod | ||
) | const |
Utility method to find geodesic between P and Q using the orthogonal method, with initial direction tPhint and initial length lengthHint.
|
inline |
This signature makes a guess at the initial direction and length and then calls the other signature.
Vec2 SimTK::ContactGeometry::calcSplitGeodError | ( | const Vec3 & | P, |
const Vec3 & | Q, | ||
const UnitVec3 & | tP, | ||
const UnitVec3 & | tQ, | ||
Geodesic * | geod = 0 |
||
) | const |
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.
void SimTK::ContactGeometry::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 at the given length.
Only possible for a few simple shapes, such as spheres and cylinders.
[in] | xP | Coordinates of the starting point for the geodesic. |
[in] | tP | The starting tangent direction for the geodesic. |
[in] | terminatingLength | The length that the resulting geodesic should have. |
[in] | options | Parameters related to geodesic calculation |
[out] | geod | On exit, this contains the calculated geodesic |
void SimTK::ContactGeometry::shootGeodesicInDirectionUntilPlaneHitAnalytical | ( | const Vec3 & | xP, |
const UnitVec3 & | tP, | ||
const Plane & | terminatingPlane, | ||
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.
[in] | xP | Coordinates of the starting point for the geodesic. |
[in] | tP | The starting tangent direction for the geodesic. |
[in] | terminatingPlane | The plane in which the end point of the resulting geodesic should lie. |
[in] | options | Parameters related to geodesic calculation |
[out] | geod | On exit, this contains the calculated geodesic |
void SimTK::ContactGeometry::calcGeodesicAnalytical | ( | const Vec3 & | xP, |
const Vec3 & | xQ, | ||
const Vec3 & | tPhint, | ||
const Vec3 & | tQhint, | ||
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.
Vec2 SimTK::ContactGeometry::calcSplitGeodErrorAnalytical | ( | const Vec3 & | P, |
const Vec3 & | Q, | ||
const UnitVec3 & | tP, | ||
const UnitVec3 & | tQ, | ||
Geodesic * | geod = 0 |
||
) | 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.
const Plane& SimTK::ContactGeometry::getPlane | ( | ) | const |
Get the plane associated with the geodesic hit plane event handler.
void SimTK::ContactGeometry::setPlane | ( | const Plane & | plane | ) | const |
Set the plane associated with the geodesic hit plane event handler.
const Geodesic& SimTK::ContactGeometry::getGeodP | ( | ) | const |
Get the geodesic for access by visualizer.
const Geodesic& SimTK::ContactGeometry::getGeodQ | ( | ) | const |
Get the geodesic for access by visualizer.
const int SimTK::ContactGeometry::getNumGeodesicsShot | ( | ) | const |
Get the plane associated with the geodesic hit plane event handler.
void SimTK::ContactGeometry::addVizReporter | ( | ScheduledEventReporter * | reporter | ) | const |
Get the plane associated with the geodesic hit plane event handler.
bool SimTK::ContactGeometry::isOwnerHandle | ( | ) | const |
Internal use only.
bool SimTK::ContactGeometry::isEmptyHandle | ( | ) | const |
Internal use only.
|
inline |
Internal use only.
|
inline |
Internal use only.
|
inline |
Internal use only.
|
protected |
Internal use only.