/usr/include/simbody/simmath/internal/ContactGeometry.h is in libsimbody-dev 3.5.4+dfsg-1ubuntu2.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417 1418 1419 1420 1421 1422 1423 1424 1425 1426 1427 1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439 1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463 1464 1465 1466 1467 1468 1469 1470 1471 1472 1473 1474 1475 1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491 1492 1493 1494 1495 1496 1497 1498 1499 1500 1501 1502 1503 1504 1505 1506 1507 1508 1509 1510 1511 1512 1513 1514 1515 1516 1517 1518 1519 1520 1521 1522 1523 1524 1525 1526 1527 1528 1529 1530 1531 1532 1533 1534 1535 1536 1537 1538 1539 1540 1541 1542 1543 1544 1545 1546 1547 1548 1549 1550 1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564 1565 1566 1567 1568 1569 1570 1571 1572 1573 1574 1575 1576 | #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 {
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 {
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) {
// 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) {
// 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_
|