ugo_zapad/horror
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
Kirill Yurkin
2024-06-10 12:48:14 +03:00
commit d54c9805b3
1398 changed files with 739400 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
thirdparty/ode-0.16.5/ode/demo/Makefile.am vendored Normal file
View File

@@ -0,0 +1,75 @@
AM_CPPFLAGS = -I$(top_srcdir)/include \
-I$(top_builddir)/include \
-DDRAWSTUFF_TEXTURE_PATH="\"$(abs_top_srcdir)/drawstuff/textures\""
if X11
AM_LDFLAGS = $(X_PRE_LIBS) $(X_LIBS) $(X_EXTRA_LIBS)
endif
# On Windows, GL_LIBS must go after libdrawstuff.la.
LDADD = $(top_builddir)/drawstuff/src/libdrawstuff.la \
$(top_builddir)/ode/src/libode.la @GL_LIBS@
noinst_HEADERS = basket_geom.h bunny_geom.h convex_bunny_geom.h convex_prism.h \
icosahedron_geom.h halton235_geom.h texturepath.h world_geom3.h
AM_DEFAULT_SOURCE_EXT = .cpp
noinst_PROGRAMS = \
demo_boxstack \
demo_buggy \
demo_cards \
demo_chain1 \
demo_chain2 \
demo_collision \
demo_convex \
demo_crash \
demo_cylvssphere \
demo_dball \
demo_dhinge \
demo_transmission \
demo_feedback \
demo_friction \
demo_gyroscopic \
demo_gyro2 \
demo_heightfield \
demo_hinge \
demo_I \
demo_jointPR \
demo_joints \
demo_jointPU \
demo_kinematic \
demo_motion \
demo_motor \
demo_ode \
demo_piston \
demo_plane2d \
demo_rfriction \
demo_slider \
demo_space \
demo_space_stress \
demo_step \
demo_tracks
demo_chain1_SOURCES = demo_chain1.c
demo_chain1_LDADD = $(LDADD) -lstdc++
if TRIMESH
noinst_PROGRAMS += \
demo_basket \
demo_cyl \
demo_moving_trimesh \
demo_moving_convex \
demo_trimesh
AM_CPPFLAGS += -DdTRIMESH_ENABLED
endif
if WIN32
resources.o: $(top_srcdir)/drawstuff/src/resources.rc $(top_srcdir)/drawstuff/src/resource.h
@WINDRES@ $(top_srcdir)/drawstuff/src/resources.rc -o resources.o
LDADD += resources.o
endif

1133
thirdparty/ode-0.16.5/ode/demo/Makefile.in vendored Normal file
View File

File diff suppressed because it is too large Load Diff

599
thirdparty/ode-0.16.5/ode/demo/basket_geom.h vendored Normal file
View File

File diff suppressed because one or more lines are too long

1366
thirdparty/ode-0.16.5/ode/demo/bunny_geom.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

468
thirdparty/ode-0.16.5/ode/demo/convex_bunny_geom.h vendored Normal file
View File

@@ -0,0 +1,468 @@
const unsigned int convexBunnyPlaneCount = 176;
dReal convexBunnyPlanes[] =
{
0.986167, -0.0612533, -0.154021, 0.399481,
0.982735, -0.0691036, -0.171628, 0.409884,
-0.984387, -0.0582774, -0.166089, 0.403079,
0.985044, -0.172279, 0.00302105, 0.437531,
0.976915, -0.184361, 0.107929, 0.465334,
0.951478, -0.281619, 0.124019, 0.475223,
0.798136, -0.502214, 0.332805, 0.535555,
0.949728, -0.211128, -0.231176, 0.528156,
-0.000894561, -0.995066, -0.0992088, 0.80299,
-0.000896015, -0.995066, -0.0992131, 0.802991,
-0.0035709, -0.935822, 0.352454, 0.618504,
-0.291551, -0.828515, 0.478081, 0.681579,
-0.978715, -0.181408, 0.0959605, 0.468907,
-0.985523, -0.169302, -0.00904739, 0.441129,
-0.953768, -0.278749, 0.11236, 0.478704,
0.372745, -0.827499, 0.419888, 0.630174,
0.976911, -0.18316, 0.109991, 0.466086,
0.817827, -0.387738, 0.425227, 0.569153,
-0.978732, -0.180211, 0.0980152, 0.469656,
0.662794, -0.0277654, 0.748287, 0.803459,
0.00359857, -0.660581, -0.750746, 0.877267,
0.00359952, -0.660579, -0.750748, 0.877266,
-0.947456, -0.208272, -0.242797, 0.531628,
-0.980764, -0.0661375, -0.18365, 0.413468,
-0.940835, -0.0698657, -0.331585, 0.494827,
0.983751, 0.0529367, -0.171556, 0.403533,
0.981839, 0.0996302, -0.16145, 0.410144,
0.977938, 0.0857834, -0.190468, 0.411823,
0.959636, 0.106068, -0.260476, 0.44898,
0.85334, -0.0495414, -0.518996, 0.60489,
-0.803667, -0.499793, 0.322995, 0.538478,
-0.689742, -0.615484, 0.381361, 0.574754,
-0.380353, -0.826364, 0.415277, 0.63155,
-0.39985, -0.817283, 0.414933, 0.630682,
-0.380309, -0.826285, 0.415473, 0.631677,
-0.981411, 0.0559147, -0.183592, 0.407121,
-0.979526, 0.101914, -0.173615, 0.413635,
-0.975381, 0.0881975, -0.202119, 0.415257,
0.988445, 0.140182, 0.0576755, 0.485174,
0.876515, 0.0408992, 0.479634, 0.620093,
0.848907, -0.0996824, 0.519057, 0.631268,
0.895754, -0.195088, 0.399457, 0.563346,
0.861448, -0.256989, 0.438023, 0.574909,
0.775672, -0.447481, 0.445076, 0.586422,
0.683157, -0.617557, 0.389768, 0.572247,
0.391755, -0.818722, 0.419789, 0.629264,
0.28317, -0.829384, 0.481599, 0.680529,
0.3727, -0.827422, 0.420079, 0.630298,
-0.824143, -0.385255, 0.415171, 0.57215,
-0.782413, -0.445128, 0.435536, 0.589267,
0.00152876, 0.999994, -0.00308275, 0.665646,
0.00242466, 0.999989, -0.0038994, 0.665879,
-0.979892, 0.121321, -0.158406, 0.420287,
0.767537, -0.190214, -0.612132, 0.695479,
0.372649, -0.42747, -0.823652, 0.869878,
0.537245, -0.335515, -0.77382, 0.82472,
0.0263648, -0.598975, -0.800334, 0.873623,
0.00393345, -0.60959, -0.792707, 0.869865,
-0.0183706, -0.598907, -0.800608, 0.873704,
0.00875728, 0.676014, -0.736836, 0.825597,
0.852333, -0.0355955, -0.521786, 0.607886,
0.392036, 0.534934, -0.748434, 0.818042,
0.847696, -0.122973, -0.516033, 0.615814,
0.884763, -0.0760716, -0.45979, 0.565893,
0.9446, -0.0727144, -0.320069, 0.491401,
0.904971, -0.0675211, -0.420081, 0.541673,
-0.899959, -0.0647928, -0.431134, 0.544968,
-0.955972, 0.108494, -0.272667, 0.452769,
-0.363823, -0.426358, -0.828161, 0.871222,
-0.528689, -0.333936, -0.780368, 0.826685,
0.982068, 0.118277, -0.146811, 0.416542,
0.98951, 0.144455, 0.00164104, 0.468616,
0.50797, -0.0708041, 0.85846, 0.883126,
0.748614, -0.431275, 0.503565, 0.634026,
0.214863, -0.405791, 0.888351, 0.94048,
-0.901162, -0.192379, 0.388455, 0.566627,
-0.867521, -0.254376, 0.427435, 0.578065,
-0.226957, -0.405135, 0.885639, 0.941284,
-0.756029, -0.429007, 0.494341, 0.636765,
-0.00629553, -0.188362, 0.982079, 0.968197,
0.0165684, 0.999846, -0.00581961, 0.670373,
0.00313267, 0.999987, -0.00405124, 0.666103,
0.545069, 0.472158, -0.692796, 0.780052,
0.932011, 0.148856, -0.330451, 0.498417,
0.844043, 0.227673, -0.485547, 0.599903,
-0.019033, 0.999801, -0.00590978, 0.672252,
-0.959662, 0.239262, -0.147656, 0.488538,
0.00151234, 0.999999, -0.000399466, 0.665855,
-0.988649, 0.143168, 0.0455675, 0.488784,
-0.989015, 0.147444, -0.01048, 0.472227,
-0.972439, 0.232727, -0.01415, 0.518344,
0.587681, -0.160147, -0.793085, 0.823999,
0.640479, -0.269179, -0.719256, 0.778142,
0.541109, -0.332896, -0.772257, 0.823226,
0.546185, -0.14771, -0.824538, 0.84854,
0.528519, -0.026044, -0.848522, 0.873136,
0.447231, -0.0756684, -0.891212, 0.903953,
0.490619, -0.0795123, -0.867739, 0.884255,
0.279393, 0.264257, -0.923097, 0.950045,
0.374653, 0.39486, -0.83888, 0.886593,
0.00050174, 0.999994, -0.00331563, 0.665976,
-0.0103777, 0.999934, -0.00487936, 0.669494,
-0.927571, 0.150741, -0.341889, 0.501807,
-0.267268, 0.265084, -0.926444, 0.951043,
-0.845984, -0.0330207, -0.532184, 0.610986,
-0.518165, -0.0244575, -0.854931, 0.875047,
-0.83753, 0.228953, -0.496108, 0.603116,
-0.535666, 0.472122, -0.700116, 0.78259,
-0.381083, 0.534649, -0.754272, 0.820328,
-0.363157, 0.395975, -0.843398, 0.88794,
-0.326829, -0.256634, -0.909572, 0.922946,
0.394875, -0.128601, -0.90969, 0.920236,
0.337169, -0.257642, -0.905504, 0.921733,
0.398433, -0.193767, -0.896496, 0.910015,
-0.536477, -0.146072, -0.831177, 0.850523,
-0.436512, -0.0743406, -0.896622, 0.905564,
-0.480187, -0.0780522, -0.873687, 0.886029,
-0.384093, -0.127432, -0.914458, 0.921656,
-0.388009, -0.192572, -0.901313, 0.911451,
0.977045, 0.15796, 0.14294, 0.521934,
0.930035, 0.231515, 0.28537, 0.600301,
-0.855499, -0.0971121, 0.508616, 0.634376,
-0.875419, 0.136849, 0.463589, 0.62897,
-0.882196, 0.0435387, 0.468864, 0.623303,
0.0204398, -0.0238739, 0.999506, 0.958419,
-0.0062197, -0.0777937, 0.99695, 0.962769,
0.907123, 0.250746, 0.338015, 0.623205,
0.902358, 0.173321, 0.394601, 0.607696,
0.870085, 0.134211, 0.474278, 0.625782,
0.0015108, 0.999999, 6.34978e-06, 0.665945,
0.00150567, 0.999999, 0.000568537, 0.666143,
0.00150738, 0.999999, 0.000565012, 0.666141,
-0.963954, 0.266039, -0.00405118, 0.539571,
0.0015136, 0.999999, -0.000393102, 0.665856,
0.00151117, 0.999999, 4.32104e-06, 0.665944,
0.0272711, 0.999604, -0.00696439, 0.673709,
0.962047, 0.236383, -0.136341, 0.484917,
0.973236, 0.229796, -0.00223071, 0.514797,
0.964728, 0.263133, 0.00776342, 0.536054,
-0.906596, 0.176056, 0.383521, 0.610999,
-0.978237, 0.160918, 0.130987, 0.525513,
-0.910434, 0.253486, 0.326887, 0.626522,
-0.932759, 0.234321, 0.27396, 0.603697,
0.800621, -0.0921333, -0.592045, 0.665278,
0.679569, -0.15358, -0.717356, 0.765121,
0.684928, -0.199829, -0.700672, 0.756959,
-0.532604, -0.33128, -0.778837, 0.82519,
-0.578395, -0.158384, -0.800234, 0.826134,
-0.671209, -0.151537, -0.725613, 0.767576,
-0.00530287, 0.323551, 0.946196, 0.94547,
-0.719766, 0.229227, 0.655281, 0.774388,
-0.604194, 0.29171, 0.741522, 0.841564,
-0.544989, 0.302925, 0.781808, 0.866398,
-0.518662, -0.0692529, 0.85217, 0.884999,
-0.671987, -0.0257512, 0.740115, 0.805898,
-0.00613626, -0.00823685, 0.999947, 0.957141,
-0.032747, -0.0237908, 0.99918, 0.958516,
-0.00530611, 0.323546, 0.946198, 0.945471,
-0.545061, 0.302657, 0.781861, 0.866377,
-0.639902, 0.23832, 0.730568, 0.823722,
0.00149706, 0.999997, 0.00193434, 0.667038,
0.00149731, 0.999997, 0.00193252, 0.667037,
-0.0048341, 0.44008, 0.897946, 0.936327,
-0.00483143, 0.440078, 0.897947, 0.936327,
-0.632454, -0.267241, -0.727039, 0.780455,
-0.67691, -0.197765, -0.709, 0.759436,
-0.841667, -0.120432, -0.526396, 0.618913,
-0.760706, -0.187792, -0.621337, 0.698143,
-0.87929, -0.0734062, -0.470597, 0.569116,
-0.847068, -0.0469762, -0.529405, 0.607991,
-0.793572, -0.0897399, -0.601823, 0.668201,
0.536355, 0.301024, 0.788485, 0.864403,
0.536284, 0.301291, 0.788431, 0.864424,
0.595945, 0.289897, 0.748872, 0.839373,
0.631626, 0.236397, 0.738353, 0.8214,
0.712378, 0.227061, 0.664049, 0.771772,
};
const unsigned int convexBunnyPointCount = 105;
dReal convexBunnyPoints[] =
{
-0.459488, -0.093017, -0.311341,
0.466635, -0.094416, -0.305669,
-0.309239, 0.776868, 0.304726,
-0.004458, -0.042526, 1.01567,
8.40779e-45, 3.00321e-39, 2.8026e-44,
0.007957, 0.282241, -0.93168,
0.204445, -0.66438, 0.513353,
-0.303961, 0.054199, 0.625921,
0.265619, 0.756464, 0.504187,
-0.402162, 0.133528, -0.443247,
8.40779e-45, 3.00321e-39, 2.8026e-44,
-0.266772, 0.64233, 0.602061,
8.40779e-45, 3.00321e-39, 2.8026e-44,
8.40779e-45, 3.00321e-39, 2.8026e-44,
0.411612, 0.132299, -0.438264,
0.31148, 0.775931, 0.308527,
0.300086, 0.053287, 0.62962,
-0.414624, 0.164083, -0.278254,
-0.248382, 0.255825, -0.627493,
-0.216201, -0.126776, -0.886936,
0.267564, -0.666174, -0.654834,
-0.135892, -0.03552, 0.945455,
-0.265837, 0.757267, 0.500933,
-0.003873, 0.161605, 0.970499,
8.40779e-45, 3.00321e-39, 2.8026e-44,
-0.282599, -0.663393, 0.412411,
0.007237, 0.361687, -0.794439,
0.093627, 0.258494, -0.920589,
0.422146, 0.162819, -0.27313,
0.279163, -0.664604, 0.417328,
0.263086, 0.512567, 0.637832,
-0.099875, 0.310931, -0.799381,
-0.446838, -0.118517, -0.466159,
-0.168842, 0.102387, -0.920381,
0.455805, -0.119881, -0.460632,
0.337743, -0.666396, -0.074503,
-0.134547, -0.119852, -0.959004,
-0.183807, 0.19697, 0.84448,
0.264969, 0.641527, 0.605317,
-0.209063, -0.663393, 0.509344,
-0.364126, -0.200299, 0.202388,
-0.253475, -0.081797, 0.756541,
0.260471, 0.255056, -0.624378,
0.114248, 0.310608, -0.79807,
0.364663, -0.201399, 0.20685,
0.127847, -0.035919, 0.94707,
8.40779e-45, 3.00321e-39, 2.8026e-44,
-0.381071, -0.629723, -0.350777,
-0.339884, -0.04115, -0.668211,
-0.077913, 0.258753, -0.92164,
0.184061, 0.101854, -0.91822,
-0.335166, -0.66538, -0.078623,
0.386561, -0.625221, -0.21687,
8.40779e-45, 3.00321e-39, 2.8026e-44,
-0.241585, 0.527592, 0.669296,
-0.086969, 0.133224, 0.947633,
-0.003127, 0.28407, 0.87887,
-0.004433, -0.146642, 0.985872,
8.40779e-45, 3.00321e-39, 2.8026e-44,
-0.138444, -0.10425, 0.945975,
-0.265676, 0.513366, 0.634594,
8.40779e-45, 3.00321e-39, 2.8026e-44,
0.247593, -0.082554, 0.75961,
0.07941, 0.132973, 0.948652,
0.238615, 0.526867, 0.672237,
8.40779e-45, 3.00321e-39, 2.8026e-44,
8.40779e-45, 3.00321e-39, 2.8026e-44,
-0.382112, -0.62406, -0.221577,
-0.104072, 0.177278, -0.95253,
0.351567, -0.042194, -0.663976,
0.138234, -0.293905, -0.897958,
0.119916, 0.17694, -0.951159,
-0.371322, -0.665382, -0.35362,
-0.263384, -0.663396, 0.466604,
0.376722, -0.666513, -0.219833,
0.387086, -0.630883, -0.346073,
-0.125544, 0.140012, 0.917678,
-0.070612, 0.036849, 0.975733,
-0.083497, -0.084934, 0.979607,
0.259286, -0.664547, 0.471281,
8.40779e-45, 3.00321e-39, 2.8026e-44,
0.074888, -0.085173, 0.980577,
0.152305, 0.125256, 0.890786,
0.130184, -0.104656, 0.94762,
-0.004249, 0.046042, 1.00324,
0.062419, 0.036648, 0.976547,
8.40779e-45, 3.00321e-39, 2.8026e-44,
-0.392666, -0.488581, -0.427494,
0.230315, -0.12745, -0.884202,
8.40779e-45, 3.00321e-39, 2.8026e-44,
0.193434, -0.665946, -0.715325,
0.007865, 0.122104, -0.956137,
8.40779e-45, 3.00321e-39, 2.8026e-44,
-0.257884, -0.665381, -0.658052,
0.377265, -0.666513, -0.349036,
-0.372362, -0.665381, -0.22442,
0.400045, -0.489778, -0.42264,
-0.159174, 0.125726, 0.888878,
0.118369, 0.139643, 0.919173,
-0.124463, -0.293508, -0.899566,
0.21172, -0.302754, -0.843303,
0.149571, -0.120281, -0.957264,
-0.183019, -0.665378, -0.71763,
0.177696, 0.196424, 0.846693,
-0.198638, -0.302135, -0.845816,
};
unsigned int convexBunnyPolygons[] =
{
3, 7, 2, 0,
3, 2, 7, 11,
3, 1, 15, 16,
3, 0, 2, 17,
3, 17, 9, 0,
3, 2, 9, 17,
3, 18, 9, 2,
3, 2, 11, 22,
3, 22, 15, 2,
3, 8, 15, 22,
3, 2, 15, 26,
3, 5, 26, 27,
3, 1, 14, 28,
3, 28, 15, 1,
3, 14, 15, 28,
3, 2, 26, 31,
3, 0, 9, 32,
3, 9, 18, 33,
3, 34, 14, 1,
3, 19, 33, 36,
3, 8, 22, 38,
3, 38, 22, 11,
3, 38, 15, 8,
3, 38, 16, 15,
3, 38, 30, 16,
3, 40, 7, 0,
3, 0, 25, 40,
3, 40, 25, 7,
3, 7, 25, 41,
3, 21, 37, 41,
3, 42, 15, 14,
3, 42, 27, 15,
3, 43, 26, 15,
3, 15, 27, 43,
3, 43, 27, 26,
3, 1, 16, 44,
3, 44, 29, 1,
3, 16, 29, 44,
3, 0, 32, 47,
3, 19, 32, 48,
3, 48, 33, 19,
3, 48, 32, 9,
3, 9, 33, 48,
3, 49, 33, 18,
3, 49, 18, 2,
3, 2, 31, 49,
3, 49, 26, 5,
3, 49, 31, 26,
3, 50, 42, 14,
3, 27, 42, 50,
3, 51, 35, 6,
3, 6, 39, 51,
3, 1, 29, 52,
3, 11, 37, 54,
3, 55, 23, 11,
3, 11, 54, 55,
3, 11, 23, 56,
3, 56, 38, 11,
3, 23, 38, 56,
3, 57, 39, 6,
3, 21, 41, 59,
3, 39, 57, 59,
3, 60, 37, 11,
3, 60, 41, 37,
3, 60, 11, 7,
3, 7, 41, 60,
3, 16, 30, 62,
3, 62, 29, 16,
3, 63, 38, 23,
3, 38, 63, 64,
3, 67, 25, 0,
3, 0, 47, 67,
3, 68, 36, 33,
3, 33, 49, 68,
3, 68, 49, 5,
3, 14, 34, 69,
3, 69, 50, 14,
3, 5, 27, 71,
3, 27, 50, 71,
3, 71, 68, 5,
3, 25, 51, 73,
3, 73, 51, 39,
3, 39, 59, 73,
3, 73, 41, 25,
3, 73, 59, 41,
3, 29, 35, 74,
3, 74, 52, 29,
3, 35, 51, 74,
3, 75, 34, 1,
3, 1, 52, 75,
3, 52, 74, 75,
3, 21, 55, 76,
3, 76, 54, 37,
3, 76, 55, 54,
3, 77, 55, 21,
3, 21, 59, 78,
3, 3, 77, 78,
3, 78, 77, 21,
3, 78, 57, 3,
3, 78, 59, 57,
3, 6, 35, 79,
3, 79, 35, 29,
3, 29, 62, 79,
3, 3, 57, 81,
3, 83, 62, 45,
3, 45, 81, 83,
3, 83, 79, 62,
3, 6, 79, 83,
3, 83, 57, 6,
3, 83, 81, 57,
3, 84, 63, 23,
3, 84, 77, 3,
3, 23, 55, 84,
3, 55, 77, 84,
3, 45, 63, 85,
3, 3, 81, 85,
3, 85, 81, 45,
3, 85, 84, 3,
3, 63, 84, 85,
3, 87, 47, 32,
3, 87, 72, 47,
3, 50, 69, 88,
3, 88, 34, 20,
3, 88, 69, 34,
3, 36, 68, 91,
3, 68, 71, 91,
3, 72, 87, 93,
3, 93, 87, 32,
3, 93, 32, 19,
3, 94, 74, 72,
3, 94, 93, 20,
3, 72, 93, 94,
3, 94, 75, 74,
3, 95, 74, 51,
3, 72, 74, 95,
3, 95, 51, 25,
3, 25, 67, 95,
3, 95, 67, 47,
3, 47, 72, 95,
3, 20, 34, 96,
3, 34, 75, 96,
3, 96, 94, 20,
3, 75, 94, 96,
3, 97, 37, 21,
3, 21, 76, 97,
3, 97, 76, 37,
3, 98, 64, 63,
3, 98, 63, 45,
3, 45, 82, 98,
3, 36, 70, 99,
3, 100, 88, 20,
3, 20, 90, 100,
3, 100, 90, 70,
3, 101, 71, 50,
3, 50, 88, 101,
3, 36, 91, 101,
3, 101, 91, 71,
3, 101, 70, 36,
3, 101, 100, 70,
3, 88, 100, 101,
3, 102, 90, 20,
3, 20, 93, 102,
3, 70, 90, 102,
3, 102, 99, 70,
3, 64, 98, 103,
3, 103, 98, 82,
3, 30, 38, 103,
3, 38, 64, 103,
3, 103, 62, 30,
3, 45, 62, 103,
3, 103, 82, 45,
3, 36, 99, 104,
3, 99, 102, 104,
3, 104, 102, 93,
3, 19, 36, 104,
3, 104, 93, 19,
};

28
thirdparty/ode-0.16.5/ode/demo/convex_prism.h vendored Normal file
View File

@@ -0,0 +1,28 @@
unsigned int prism_pointcount = 8;
unsigned int prism_planecount = 6;
dReal prism_points[24]={
10.0, 1.0,-1.0,
10.0,-1.0,-1.0,
-10.0,-1.0,-1.0,
-10.0, 1.0,-1.0,
10.0, 1.0, 1.0,
10.0,-1.0, 1.0,
-10.0,-1.0, 1.0,
-10.0, 1.0, 1.0
};
unsigned int prism_polygons[]={
4,0,1,2,3,
4,4,7,6,5,
4,0,4,5,1,
4,1,5,6,2,
4,2,6,7,3,
4,4,0,3,7,
};
dReal prism_planes[]={
0.0,0.0,-1.0,1.0,
0.0,0.0,1.0,1.0,
1.0,0.0,0.0,10.0,
0.0,-1.0,0.0,1.0,
-1.0,0.0,-0.0,10.0,
0.0,1.0,0.0,1.0,
};

253
thirdparty/ode-0.16.5/ode/demo/demo_I.cpp vendored Normal file
View File

@@ -0,0 +1,253 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
test that the rotational physics is correct.
an "anchor body" has a number of other randomly positioned bodies
("particles") attached to it by ball-and-socket joints, giving it some
random effective inertia tensor. the effective inertia matrix is calculated,
and then this inertia is assigned to another "test" body. a random torque is
applied to both bodies and the difference in angular velocity and orientation
is observed after a number of iterations.
typical errors for each test cycle are about 1e-5 ... 1e-4.
*/
#include <time.h>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define NUM 10 // number of particles
#define SIDE 0.1 // visual size of the particles
// dynamics objects an globals
static dWorldID world=0;
static dBodyID anchor_body,particle[NUM],test_body;
static dJointID particle_joint[NUM];
static dReal torque[3];
static int iteration;
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {1.5572f,-1.8886f,1.5700f};
static float hpr[3] = {118.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
// compute the mass parameters of a particle set. q = particle positions,
// pm = particle masses
#define _I(i,j) I[(i)*4+(j)]
void computeMassParams (dMass *m, dReal q[NUM][3], dReal pm[NUM])
{
int i,j;
dMassSetZero (m);
for (i=0; i<NUM; i++) {
m->mass += pm[i];
for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j];
m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]);
m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]);
m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]);
m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]);
m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]);
m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]);
}
for (j=0; j<3; j++) m->c[j] /= m->mass;
m->_I(1,0) = m->_I(0,1);
m->_I(2,0) = m->_I(0,2);
m->_I(2,1) = m->_I(1,2);
}
void reset_test()
{
int i;
dMass m,anchor_m;
dReal q[NUM][3], pm[NUM]; // particle positions and masses
dReal pos1[3] = {1,0,1}; // point of reference (POR)
dReal pos2[3] = {-1,0,1}; // point of reference (POR)
// make random particle positions (relative to POR) and masses
for (i=0; i<NUM; i++) {
pm[i] = dRandReal()+0.1;
q[i][0] = dRandReal()-0.5;
q[i][1] = dRandReal()-0.5;
q[i][2] = dRandReal()-0.5;
}
// adjust particle positions so centor of mass = POR
computeMassParams (&m,q,pm);
for (i=0; i<NUM; i++) {
q[i][0] -= m.c[0];
q[i][1] -= m.c[1];
q[i][2] -= m.c[2];
}
if (world) dWorldDestroy (world);
world = dWorldCreate();
anchor_body = dBodyCreate (world);
dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
dMassAdjust (&anchor_m,0.1);
dBodySetMass (anchor_body,&anchor_m);
for (i=0; i<NUM; i++) {
particle[i] = dBodyCreate (world);
dBodySetPosition (particle[i],
pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,pm[i]);
dBodySetMass (particle[i],&m);
}
for (i=0; i < NUM; i++) {
particle_joint[i] = dJointCreateBall (world,0);
dJointAttach (particle_joint[i],anchor_body,particle[i]);
const dReal *p = dBodyGetPosition (particle[i]);
dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
}
// make test_body with the same mass and inertia of the anchor_body plus
// all the particles
test_body = dBodyCreate (world);
dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
computeMassParams (&m,q,pm);
m.mass += anchor_m.mass;
for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
dBodySetMass (test_body,&m);
// rotate the test and anchor bodies by a random amount
dQuaternion qrot;
for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
dNormalize4 (qrot);
dBodySetQuaternion (anchor_body,qrot);
dBodySetQuaternion (test_body,qrot);
dMatrix3 R;
dQtoR (qrot,R);
for (i=0; i<NUM; i++) {
dVector3 v;
dMultiply0 (v,R,&q[i][0],3,3,1);
dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
}
// set random torque
for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;
iteration=0;
}
// simulation loop
static void simLoop (int pause)
{
if (!pause) {
dBodyAddTorque (anchor_body,torque[0],torque[1],torque[2]);
dBodyAddTorque (test_body,torque[0],torque[1],torque[2]);
dWorldStep (world,0.03);
iteration++;
if (iteration >= 100) {
// measure the difference between the anchor and test bodies
const dReal *w1 = dBodyGetAngularVel (anchor_body);
const dReal *w2 = dBodyGetAngularVel (test_body);
const dReal *q1 = dBodyGetQuaternion (anchor_body);
const dReal *q2 = dBodyGetQuaternion (test_body);
dReal maxdiff = dMaxDifference (w1,w2,1,3);
printf ("w-error = %.4e (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n",
maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]);
maxdiff = dMaxDifference (q1,q2,1,4);
printf ("q-error = %.4e\n",maxdiff);
reset_test();
}
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE};
dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE};
dsSetColor (1,1,1);
dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body),
sides3);
dsSetColor (1,0,0);
dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2);
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawBox (dBodyGetPosition (particle[i]),
dBodyGetRotation (particle[i]), sides);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dInitODE2(0);
dRandSetSeed (time(0));
reset_test();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}

276
thirdparty/ode-0.16.5/ode/demo/demo_basket.cpp vendored Normal file
View File

@@ -0,0 +1,276 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// Basket ball demo.
// Serves as a test for the sphere vs trimesh collider
// By Bram Stolk.
// Press the spacebar to reset the position of the ball.
#include <assert.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#include "basket_geom.h" // this is our world mesh
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// some constants
#define RADIUS 0.14
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID sphbody;
static dGeomID sphgeom;
static dJointGroupID contactgroup;
static dGeomID world_mesh;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
// fprintf(stderr,"testing geoms %p %p\n", o1, o2);
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
// Paranoia <-- not working for some people, temporarily removed for 0.6
//dIASSERT(dVALIDVEC3(contact[i].geom.pos));
//dIASSERT(dVALIDVEC3(contact[i].geom.normal));
//dIASSERT(!dIsNan(contact[i].geom.depth));
contact[i].surface.slip1 = 0.7;
contact[i].surface.slip2 = 0.7;
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
contact[i].surface.mu = 50.0; // was: dInfinity
contact[i].surface.soft_erp = 0.96;
contact[i].surface.soft_cfm = 0.04;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {-8,0,5};
static float hpr[3] = {0.0f,-29.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
static void reset_ball(void)
{
float sx=0.0f, sy=3.40f, sz=7.15;
dQuaternion q;
dQSetIdentity(q);
dBodySetPosition (sphbody, sx, sy, sz);
dBodySetQuaternion(sphbody, q);
dBodySetLinearVel (sphbody, 0,0,0);
dBodySetAngularVel (sphbody, 0,0,0);
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd)
{
case ' ':
reset_ball();
break;
}
}
// simulation loop
static void simLoop (int pause)
{
double simstep = 0.001; // 1ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf(dt/simstep);
// fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps);
for (int i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,1,1);
const dReal *SPos = dBodyGetPosition(sphbody);
const dReal *SRot = dBodyGetRotation(sphbody);
float spos[3] = {SPos[0], SPos[1], SPos[2]};
float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
dsDrawSphere
(
spos,
srot,
RADIUS
);
// draw world trimesh
dsSetColor(0.4,0.7,0.9);
dsSetTexture (DS_NONE);
const dReal* Pos = dGeomGetPosition(world_mesh);
//dIASSERT(dVALIDVEC3(Pos));
float pos[3] = { Pos[0], Pos[1], Pos[2] };
const dReal* Rot = dGeomGetRotation(world_mesh);
//dIASSERT(dVALIDMAT3(Rot));
float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };
int numi = sizeof(world_indices) / sizeof(dTriIndex);
for (int i=0; i<numi/3; i++)
{
int i0 = world_indices[i*3+0];
int i1 = world_indices[i*3+1];
int i2 = world_indices[i*3+2];
float *v0 = world_vertices+i0*3;
float *v1 = world_vertices+i1*3;
float *v2 = world_vertices+i2*3;
dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
}
}
int main (int argc, char **argv)
{
dMass m;
dMatrix3 R;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 64);
// Create a static world using a triangle mesh that we can collide with.
int numv = sizeof(world_vertices)/(3*sizeof(float));
int numi = sizeof(world_indices)/ sizeof(dTriIndex);
printf("numv=%d, numi=%d\n", numv, numi);
dTriMeshDataID Data = dGeomTriMeshDataCreate();
// fprintf(stderr,"Building Single Precision Mesh\n");
dGeomTriMeshDataBuildSingle
(
Data,
world_vertices,
3 * sizeof(float),
numv,
world_indices,
numi,
3 * sizeof(dTriIndex)
);
world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
dGeomTriMeshEnableTC(world_mesh, dSphereClass, false);
dGeomTriMeshEnableTC(world_mesh, dBoxClass, false);
dGeomSetPosition(world_mesh, 0, 0, 0.5);
dRSetIdentity(R);
//dIASSERT(dVALIDMAT3(R));
dGeomSetRotation (world_mesh, R);
//float sx=0.0, sy=3.40, sz=6.80;
(void)world_normals; // get rid of compiler warning
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,RADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, RADIUS);
dGeomSetBody (sphgeom,sphbody);
reset_ball();
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// Causes segm violation? Why?
// (because dWorldDestroy() destroys body connected to geom; must call first!)
dGeomDestroy(sphgeom);
dGeomDestroy (world_mesh);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

619
thirdparty/ode-0.16.5/ode/demo/demo_boxstack.cpp vendored Normal file
View File

@@ -0,0 +1,619 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
#include "icosahedron_geom.h"
//<---- Convex Object
dReal planes[]= // planes for a cube, these should coincide with the face array
{
1.0f ,0.0f ,0.0f ,0.25f,
0.0f ,1.0f ,0.0f ,0.25f,
0.0f ,0.0f ,1.0f ,0.25f,
-1.0f,0.0f ,0.0f ,0.25f,
0.0f ,-1.0f,0.0f ,0.25f,
0.0f ,0.0f ,-1.0f,0.25f
/*
1.0f ,0.0f ,0.0f ,2.0f,
0.0f ,1.0f ,0.0f ,1.0f,
0.0f ,0.0f ,1.0f ,1.0f,
0.0f ,0.0f ,-1.0f,1.0f,
0.0f ,-1.0f,0.0f ,1.0f,
-1.0f,0.0f ,0.0f ,0.0f
*/
};
const unsigned int planecount=6;
dReal points[]= // points for a cube
{
0.25f,0.25f,0.25f, // point 0
-0.25f,0.25f,0.25f, // point 1
0.25f,-0.25f,0.25f, // point 2
-0.25f,-0.25f,0.25f,// point 3
0.25f,0.25f,-0.25f, // point 4
-0.25f,0.25f,-0.25f,// point 5
0.25f,-0.25f,-0.25f,// point 6
-0.25f,-0.25f,-0.25f,// point 7
};
const unsigned int pointcount=8;
unsigned int polygons[] = //Polygons for a cube (6 squares)
{
4,0,2,6,4, // positive X
4,1,0,4,5, // positive Y
4,0,1,3,2, // positive Z
4,3,1,5,7, // negative X
4,2,3,7,6, // negative Y
4,5,4,6,7, // negative Z
};
//----> Convex Object
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawConvex dsDrawConvexD
#endif
// some constants
#define NUM 100 // max number of objects
#define DENSITY (5.0) // density of all objects
#define GPB 3 // maximum number of geometries per body
#define MAX_CONTACTS 8 // maximum number of contact points per body
#define MAX_FEEDBACKNUM 20
#define GRAVITY REAL(0.5)
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom[GPB]; // geometries representing this body
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1; // selected object
static int show_aabb = 0; // show geom AABBs?
static int show_contacts = 0; // show contact points?
static int random_pos = 1; // drop objects from random position?
static int write_world = 0;
static int show_body = 0;
struct MyFeedback {
dJointFeedback fb;
bool first;
};
static int doFeedback=0;
static MyFeedback feedbacks[MAX_FEEDBACKNUM];
static int fbnum=0;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact))
return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i<MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts) {
dsSetColor(0,0,1);
dsDrawBox(contact[i].geom.pos,RI,ss);
}
if (doFeedback && (b1==obj[selected].body || b2==obj[selected].body)) {
if (fbnum<MAX_FEEDBACKNUM) {
feedbacks[fbnum].first = b1==obj[selected].body;
dJointSetFeedback(c,&feedbacks[fbnum++].fb);
}
else fbnum++;
}
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("To drop another object, press:\n");
printf (" b for box.\n");
printf (" s for sphere.\n");
printf (" c for capsule.\n");
printf (" y for cylinder.\n");
printf (" v for a convex object.\n");
printf (" x for a composite object.\n");
printf ("To select an object, press space.\n");
printf ("To disable the selected object, press d.\n");
printf ("To enable the selected object, press e.\n");
printf ("To dump transformation data for the selected object, press p.\n");
printf ("To toggle showing the geom AABBs, press a.\n");
printf ("To toggle showing the contact points, press t.\n");
printf ("To toggle dropping from random position/orientation, press r.\n");
printf ("To save the current state to 'state.dif', press 1.\n");
printf ("To show joint feedbacks of selected object, press f.\n");
}
static char locase(char c)
{
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void command(int cmd)
{
dsizeint i;
int j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase(cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') {
if (num < NUM) {
// new object to be created
i = num;
num++;
} else {
// recycle existing object
i = nextobj++;
nextobj %= num; // wrap-around if needed
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
obj[i].body = 0;
for (k=0; k < GPB; k++)
if (obj[i].geom[k]) {
dGeomDestroy(obj[i].geom[k]);
obj[i].geom[k] = 0;
}
}
obj[i].body = dBodyCreate(world);
for (k=0; k<3; k++)
sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition(obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
} else {
// higher than highest body position
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition(obj[k].body);
if (pos[2] > maxheight)
maxheight = pos[2];
}
dBodySetPosition(obj[i].body, 0,0,maxheight+1);
dRSetIdentity(R);
//dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
}
dBodySetRotation(obj[i].body,R);
if (cmd == 'b') {
dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]);
} else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
} else if (cmd == 'v') {
dMassSetBox(&m,DENSITY,0.25,0.25,0.25);
#if 0
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
#else
obj[i].geom[0] = dCreateConvex(space,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
} else if (cmd == 'y') {
dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]);
} else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
} else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num)
selected = 0;
if (selected == -1)
selected = 0;
} else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable(obj[selected].body);
} else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable(obj[selected].body);
} else if (cmd == 'a') {
show_aabb = !show_aabb;
} else if (cmd == 't') {
show_contacts = !show_contacts;
} else if (cmd == 'r') {
random_pos = !random_pos;
} else if (cmd == '1') {
write_world = 1;
} else if (cmd == 'p'&& selected >= 0) {
const dReal* pos = dGeomGetPosition(obj[selected].geom[0]);
const dReal* rot = dGeomGetRotation(obj[selected].geom[0]);
printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]);
printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n",
rot[0],rot[1],rot[2],rot[3],
rot[4],rot[5],rot[6],rot[7],
rot[8],rot[9],rot[10],rot[11]);
} else if (cmd == 'f' && selected >= 0 && selected < num) {
if (dBodyIsEnabled(obj[selected].body))
doFeedback = 1;
}
}
// draw a geom
void drawGeom(dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (!g)
return;
if (!pos)
pos = dGeomGetPosition(g);
if (!R)
R = dGeomGetRotation(g);
int type = dGeomGetClass(g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox(pos,R,sides);
} else if (type == dSphereClass) {
dsDrawSphere(pos,R,dGeomSphereGetRadius(g));
} else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams(g,&radius,&length);
dsDrawCapsule(pos,R,length,radius);
} else if (type == dConvexClass) {
#if 0
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
#else
dsDrawConvex(pos,R,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
} else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams(g,&radius,&length);
dsDrawCylinder(pos,R,length,radius);
}
if (show_body) {
dBodyID body = dGeomGetBody(g);
if (body) {
const dReal *bodypos = dBodyGetPosition(body);
const dReal *bodyr = dBodyGetRotation(body);
dReal bodySides[3] = { 0.1, 0.1, 0.1 };
dsSetColorAlpha(0,1,0,1);
dsDrawBox(bodypos,bodyr,bodySides);
}
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB(g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++)
bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++)
bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha(1,0,0,0.5);
dsDrawBox(bbpos,RI,bbsides);
}
}
// simulation loop
static void simLoop(int pause)
{
dSpaceCollide(space, 0, &nearCallback);
if (!pause)
dWorldQuickStep(world, 0.02);
if (write_world) {
FILE *f = fopen("state.dif","wt");
if (f) {
dWorldExportDIF(world,f,"X");
fclose (f);
}
write_world = 0;
}
if (doFeedback) {
if (fbnum>MAX_FEEDBACKNUM)
printf("joint feedback buffer overflow!\n");
else {
dVector3 sum = {0, 0, 0};
printf("\n");
for (int i=0; i<fbnum; i++) {
dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2;
printf("%f %f %f\n", f[0], f[1], f[2]);
sum[0] += f[0];
sum[1] += f[1];
sum[2] += f[2];
}
printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]);
dMass m;
dBodyGetMass(obj[selected].body, &m);
printf("Object G=%f\n", GRAVITY*m.mass);
}
doFeedback = 0;
fbnum = 0;
}
// remove all contact joints
dJointGroupEmpty(contactgroup);
dsSetTexture(DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor(0,0.7,1);
} else if (!dBodyIsEnabled(obj[i].body)) {
dsSetColor(1,0.8,0);
} else {
dsSetColor(1,1,0);
}
drawGeom(obj[i].geom[j],0,0,show_aabb);
}
}
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dWorldSetGravity(world,0,0,-GRAVITY);
dWorldSetCFM(world,1e-5);
dWorldSetAutoDisableFlag(world,1);
#if 1
dWorldSetAutoDisableAverageSamplesCount( world, 10 );
#endif
dWorldSetLinearDamping(world, 0.00001);
dWorldSetAngularDamping(world, 0.005);
dWorldSetMaxAngularSpeed(world, 200);
dWorldSetContactMaxCorrectingVel(world,0.1);
dWorldSetContactSurfaceLayer(world,0.001);
dCreatePlane(space,0,0,1,0);
memset(obj,0,sizeof(obj));
dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
// dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
// run simulation
dsSimulationLoop(argc,argv,640,480,&fn);
dThreadingImplementationShutdownProcessing(threading);
dThreadingFreeThreadPool(pool);
dWorldSetStepThreadingImplementation(world, NULL, NULL);
dThreadingFreeImplementation(threading);
dJointGroupDestroy(contactgroup);
dSpaceDestroy(space);
dWorldDestroy(world);
dCloseODE();
}

308
thirdparty/ode-0.16.5/ode/demo/demo_buggy.cpp vendored Normal file
View File

@@ -0,0 +1,308 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
buggy with suspension.
this also shows you how to use geom groups.
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define LENGTH 0.7 // chassis length
#define WIDTH 0.5 // chassis width
#define HEIGHT 0.2 // chassis height
#define RADIUS 0.18 // wheel radius
#define STARTZ 0.5 // starting height of chassis
#define CMASS 1 // chassis mass
#define WMASS 0.2 // wheel mass
static const dVector3 yunit = { 0, 1, 0 }, zunit = { 0, 0, 1 };
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID body[4];
static dJointID joint[3]; // joint[0] is the front wheel
static dJointGroupID contactgroup;
static dGeomID ground;
static dSpaceID car_space;
static dGeomID box[1];
static dGeomID sphere[3];
static dGeomID ground_box;
// things that the user controls
static dReal speed=0,steer=0; // user commands
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i,n;
// only collide things with the ground
int g1 = (o1 == ground || o1 == ground_box);
int g2 = (o2 == ground || o2 == ground_box);
if (!(g1 ^ g2)) return;
const int N = 10;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0) {
for (i=0; i<n; i++) {
contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = dInfinity;
contact[i].surface.slip1 = 0.1;
contact[i].surface.slip2 = 0.1;
contact[i].surface.soft_erp = 0.5;
contact[i].surface.soft_cfm = 0.3;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press:\t'a' to increase speed.\n"
"\t'z' to decrease speed.\n"
"\t',' to steer left.\n"
"\t'.' to steer right.\n"
"\t' ' to reset speed and steering.\n"
"\t'1' to save the current state to 'state.dif'.\n");
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd) {
case 'a': case 'A':
speed += 0.3;
break;
case 'z': case 'Z':
speed -= 0.3;
break;
case ',':
steer -= 0.5;
break;
case '.':
steer += 0.5;
break;
case ' ':
speed = 0;
steer = 0;
break;
case '1': {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"");
fclose (f);
}
}
}
}
// simulation loop
static void simLoop (int pause)
{
int i;
if (!pause) {
// motor
dJointSetHinge2Param (joint[0],dParamVel2,-speed);
dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
// steering
dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
v *= 10.0;
dJointSetHinge2Param (joint[0],dParamVel,v);
dJointSetHinge2Param (joint[0],dParamFMax,0.2);
dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
dsSetColor (0,1,1);
dsSetTexture (DS_WOOD);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
dsSetColor (1,1,1);
for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
dBodyGetRotation(body[i]),0.02f,RADIUS);
dVector3 ss;
dGeomBoxGetLengths (ground_box,ss);
dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
}
int main (int argc, char **argv)
{
int i;
dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
ground = dCreatePlane (space,0,0,1,0);
// chassis body
body[0] = dBodyCreate (world);
dBodySetPosition (body[0],0,0,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[0],&m);
box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[0],body[0]);
// wheel bodies
for (i=1; i<=3; i++) {
body[i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[i],&m);
sphere[i-1] = dCreateSphere (0,RADIUS);
dGeomSetBody (sphere[i-1],body[i]);
}
dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front and back wheel hinges
for (i=0; i<3; i++) {
joint[i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[i],body[0],body[i+1]);
const dReal *a = dBodyGetPosition (body[i+1]);
dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
dJointSetHinge2Axes (joint[i], zunit, yunit);
}
// set joint suspension
for (i=0; i<3; i++) {
dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
}
// lock back wheels along the steering axis
for (i=1; i<3; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (joint[i],dParamLoStop,0);
dJointSetHinge2Param (joint[i],dParamHiStop,0);
// the following alternative method is no good as the wheels may get out
// of alignment:
// dJointSetHinge2Param (joint[i],dParamVel,0);
// dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
}
// create car space and add it to the top level space
car_space = dSimpleSpaceCreate (space);
dSpaceSetCleanup (car_space,0);
dSpaceAdd (car_space,box[0]);
dSpaceAdd (car_space,sphere[0]);
dSpaceAdd (car_space,sphere[1]);
dSpaceAdd (car_space,sphere[2]);
// environment
ground_box = dCreateBox (space,2,1.5,1);
dMatrix3 R;
dRFromAxisAndAngle (R,0,1,0,-0.15);
dGeomSetPosition (ground_box,2,0,-0.34);
dGeomSetRotation (ground_box,R);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dGeomDestroy (box[0]);
dGeomDestroy (sphere[0]);
dGeomDestroy (sphere[1]);
dGeomDestroy (sphere[2]);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

237
thirdparty/ode-0.16.5/ode/demo/demo_cards.cpp vendored Normal file
View File

@@ -0,0 +1,237 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <vector>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#endif
static int levels = 5;
static int ncards = 0;
static dSpaceID space;
static dWorldID world;
static dJointGroupID contactgroup;
struct Card {
dBodyID body;
dGeomID geom;
static const dReal sides[3];
Card()
{
body = dBodyCreate(world);
geom = dCreateBox(space, sides[0], sides[1], sides[2]);
dGeomSetBody(geom, body);
dGeomSetData(geom, this);
dMass mass;
mass.setBox(1, sides[0], sides[1], sides[2]);
dBodySetMass(body, &mass);
}
~Card()
{
dBodyDestroy(body);
dGeomDestroy(geom);
}
void draw() const
{
dsDrawBox(dBodyGetPosition(body),
dBodyGetRotation(body), sides);
}
};
static const dReal cwidth=.5, cthikness=.02, clength=1;
const dReal Card::sides[3] = { cwidth, cthikness, clength };
std::vector<Card*> cards;
int getncards(int levels)
{
return (3*levels*levels + levels) / 2;
}
void place_cards()
{
ncards = getncards(levels);
// destroy removed cards (if any)
int oldcards = cards.size();
for (int i=ncards; i<oldcards; ++i)
delete cards[i];
cards.resize(ncards);
// construct new cards (if any)
for (int i=oldcards; i<ncards; ++i)
cards[i] = new Card;
// for each level
int c = 0;
dMatrix3 right, left, hrot;
dReal angle = 20*M_PI/180.;
dRFromAxisAndAngle(right, 1, 0, 0, -angle);
dRFromAxisAndAngle(left, 1, 0, 0, angle);
dRFromAxisAndAngle(hrot, 1, 0, 0, 91*M_PI/180.);
dReal eps = 0.05;
dReal vstep = cos(angle)*clength + eps;
dReal hstep = sin(angle)*clength + eps;
for (int lvl=0; lvl<levels; ++lvl) {
// there are 3*(levels-lvl)-1 cards in each level, except last
int n = (levels-lvl);
dReal height = (lvl)*vstep + vstep/2;
// inclined cards
for (int i=0; i<2*n; ++i, ++c) {
dBodySetPosition(cards[c]->body,
0,
-n*hstep + hstep*i,
height
);
if (i%2)
dBodySetRotation(cards[c]->body, left);
else
dBodySetRotation(cards[c]->body, right);
}
if (n==1) // top of the house
break;
// horizontal cards
for (int i=0; i<n-1; ++i, ++c) {
dBodySetPosition(cards[c]->body,
0,
-(n-1 - (clength-hstep)/2)*hstep + 2*hstep*i,
height + vstep/2);
dBodySetRotation(cards[c]->body, hrot);
}
}
}
void start()
{
puts("Controls:");
puts(" SPACE - reposition cards");
puts(" - - one less level");
puts(" = - one more level");
}
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
const int MAX_CONTACTS = 8;
dContact contact[MAX_CONTACTS];
int numc = dCollide (o1, o2, MAX_CONTACTS,
&contact[0].geom,
sizeof(dContact));
for (int i=0; i<numc; i++) {
contact[i].surface.mode = dContactApprox1;
contact[i].surface.mu = 5;
dJointID c = dJointCreateContact (world, contactgroup, contact+i);
dJointAttach (c, b1, b2);
}
}
void simLoop(int pause)
{
if (!pause) {
dSpaceCollide (space, 0, &nearCallback);
dWorldQuickStep(world, 0.01);
dJointGroupEmpty(contactgroup);
}
dsSetColor (1,1,0);
for (int i=0; i<ncards; ++i) {
dsSetColor (1, dReal(i)/ncards, 0);
cards[i]->draw();
}
}
void command(int c)
{
switch (c) {
case '=':
levels++;
place_cards();
break;
case '-':
levels--;
if (levels <= 0)
levels++;
place_cards();
break;
case ' ':
place_cards();
break;
}
}
int main(int argc, char **argv)
{
dInitODE();
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
world = dWorldCreate();
dWorldSetGravity(world, 0, 0, -0.5);
dWorldSetQuickStepNumIterations(world, 50); // <-- increase for more stability
space = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dGeomID ground = dCreatePlane(space, 0, 0, 1, 0);
place_cards();
// run simulation
dsSimulationLoop (argc, argv, 640, 480, &fn);
levels = 0;
place_cards();
dJointGroupDestroy(contactgroup);
dWorldDestroy(world);
dGeomDestroy(ground);
dSpaceDestroy(space);
dCloseODE();
}

171
thirdparty/ode-0.16.5/ode/demo/demo_chain1.c vendored Normal file
View File

@@ -0,0 +1,171 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/* exercise the C interface */
#include <stdio.h>
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) /* for VC++, no precision loss complaints */
#endif
/* select correct drawing functions */
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
/* some constants */
#define NUM 10 /* number of boxes */
#define SIDE (0.2) /* side length of a box */
#define MASS (1.0) /* mass of a box */
#define RADIUS (0.1732f) /* sphere radius */
/* dynamics and collision objects */
static dWorldID world;
static dSpaceID space;
static dBodyID body[NUM];
static dJointID joint[NUM-1];
static dJointGroupID contactgroup;
static dGeomID sphere[NUM];
/* this is called by dSpaceCollide when two objects in space are
* potentially colliding.
*/
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
/* exit without doing anything if the two bodies are connected by a joint */
dBodyID b1,b2;
dContact contact;
(void)data;
b1 = dGeomGetBody(o1);
b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
contact.surface.mode = 0;
contact.surface.mu = 0.1;
contact.surface.mu2 = 0;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world,contactgroup,&contact);
dJointAttach (c,b1,b2);
}
}
/* start simulation - set viewpoint */
static void start()
{
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
}
/* simulation loop */
static void simLoop (int pause)
{
int i;
if (!pause) {
static double angle = 0;
angle += 0.05;
dBodyAddForce (body[NUM-1],0,0,1.5*(sin(angle)+1.0));
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
/* remove all contact joints */
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (i=0; i<NUM; i++) dsDrawSphere (dBodyGetPosition(body[i]),
dBodyGetRotation(body[i]),RADIUS);
}
int main (int argc, char **argv)
{
int i;
dReal k;
dMass m;
/* setup pointers to drawstuff callback functions */
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
/* create world */
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (1000000);
dWorldSetGravity (world,0,0,-0.5);
dCreatePlane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i] = dBodyCreate (world);
k = i*SIDE;
dBodySetPosition (body[i],k,k,k+0.4);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dBodySetMass (body[i],&m);
sphere[i] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[i],body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[i],body[i+1]);
k = (i+0.5)*SIDE;
dJointSetBallAnchor (joint[i],k,k,k+0.4);
}
/* run simulation */
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

165
thirdparty/ode-0.16.5/ode/demo/demo_chain2.cpp vendored Normal file
View File

@@ -0,0 +1,165 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/* exercise the C++ interface */
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define NUM 10 // number of boxes
#define SIDE (0.2) // side length of a box
#define MASS (1.0) // mass of a box
#define RADIUS (0.1732f) // sphere radius
//using namespace ode;
// dynamics and collision objects
static dWorld world;
static dSimpleSpace space (0);
static dBody body[NUM];
static dBallJoint joint[NUM-1];
static dJointGroup contactgroup;
static dBox box[NUM];
// this is called by space.collide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
// @@@ it's still more convenient to use the C interface here.
dContact contact;
contact.surface.mode = 0;
contact.surface.mu = dInfinity;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world.id(),contactgroup.id(),&contact);
dJointAttach (c,b1,b2);
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
// simulation loop
static void simLoop (int pause)
{
if (!pause) {
static double angle = 0;
angle += 0.05;
body[NUM-1].addForce (0,0,1.5*(sin(angle)+1.0));
space.collide (0,&nearCallback);
world.step (0.05);
// remove all contact joints
contactgroup.empty();
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<NUM; i++)
dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
int i;
contactgroup.create ();
world.setGravity (0,0,-0.5);
dWorldSetCFM (world.id(),1e-5);
dPlane plane (space,0,0,1,0);
for (i=0; i<NUM; i++) {
body[i].create (world);
dReal k = i*SIDE;
body[i].setPosition (k,k,k+0.4);
dMass m;
m.setBox (1,SIDE,SIDE,SIDE);
m.adjust (MASS);
body[i].setMass (&m);
body[i].setData ((void*)(dsizeint)i);
box[i].create (space,SIDE,SIDE,SIDE);
box[i].setBody (body[i]);
}
for (i=0; i<(NUM-1); i++) {
joint[i].create (world);
joint[i].attach (body[i],body[i+1]);
dReal k = (i+0.5)*SIDE;
joint[i].setAnchor (k,k,k+0.4);
}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dCloseODE();
return 0;
}

1463
thirdparty/ode-0.16.5/ode/demo/demo_collision.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

307
thirdparty/ode-0.16.5/ode/demo/demo_convex.cpp vendored Normal file
View File

@@ -0,0 +1,307 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// Convex demo.
// Serves as a test for the convex geometry.
// By Bram Stolk.
#include <assert.h>
#ifdef HAVE_UNISTD_H
# include <unistd.h>
#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#include "halton235_geom.h"
#ifdef dDOUBLE
# define dsDrawConvex dsDrawConvexD
# define dsDrawLine dsDrawLineD
#endif
#ifdef _MSC_VER
# pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// Height at which we drop the composite block.
const dReal H=4.20;
static dWorldID world;
static dSpaceID space;
static dBodyID mbody;
static dBodyID hbody[ halton_numc ];
static dGeomID hgeom[ halton_numc ];
static dJointGroupID contactgroup;
static bool drawpos=false;
static bool solidkernel=false;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.slip1 = 0.7;
contact[i].surface.slip2 = 0.7;
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
contact[i].surface.mu = 500.0; // was: dInfinity
contact[i].surface.soft_erp = 0.50;
contact[i].surface.soft_cfm = 0.03;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach
(
c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2)
);
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {-8,0,5};
static float hpr[3] = {0.0f,-29.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
fprintf(stderr,"Press SPACE to reset the simulation.\n");
}
static void reset()
{
dQuaternion q;
dQSetIdentity(q);
dBodySetPosition(mbody,0,0,0+H);
dBodySetQuaternion(mbody, q);
dBodySetLinearVel(mbody, 0,0,0);
dBodySetAngularVel(mbody, 0,0,0);
dBodyEnable(mbody);
for ( int i=0; i<halton_numc; ++i )
{
dBodyID body = hbody[i];
if ( !body ) continue;
dBodySetPosition(body, halton_pos[i][0], halton_pos[i][1], halton_pos[i][2]+H);
dBodySetQuaternion(body, q);
dBodySetLinearVel(body, 0,0,0);
dBodySetAngularVel(body, 0,0,0);
dBodyEnable(body);
}
}
// called when a key pressed
static void command(int cmd)
{
switch (cmd)
{
case ' ':
reset();
break;
default:
break;
}
}
static void simLoop(int pause)
{
double simstep = 1/240.0;
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf(dt/simstep);
nrofsteps = nrofsteps > 8 ? 8 : nrofsteps;
for (int i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,1,1);
// Draw the convex objects.
for ( int i=0; i<halton_numc; ++i )
{
dGeomID geom = hgeom[i];
dBodyID body = dGeomGetBody(geom);
//const dReal *pos = dBodyGetPosition(body);
//const dReal *rot = dBodyGetRotation(body);
const dReal *pos = dGeomGetPosition(geom);
const dReal *rot = dGeomGetRotation(geom);
dsDrawConvex
(
pos, rot,
halton_planes[i],
halton_numf[i],
halton_verts[i],
halton_numv[i],
halton_faces[i]
);
}
if (drawpos)
{
dsSetColor(1,0,0.2);
dsSetTexture(DS_NONE);
const dReal l = 0.35;
for ( int i=0; i<halton_numc; ++i )
{
dBodyID body = hbody[i];
const dReal *pos = dBodyGetPosition(body);
dReal x0[3] = { pos[0]-l, pos[1], pos[2] };
dReal x1[3] = { pos[0]+l, pos[1], pos[2] };
dReal y0[3] = { pos[0], pos[1]-l, pos[2] };
dReal y1[3] = { pos[0], pos[1]+l, pos[2] };
dReal z0[3] = { pos[0], pos[1], pos[2]-l };
dReal z1[3] = { pos[0], pos[1], pos[2]+l };
dsDrawLine(x0,x1);
dsDrawLine(y0,y1);
dsDrawLine(z0,z1);
}
}
}
int main (int argc, char **argv)
{
dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
dHashSpaceSetLevels(space, -3, 5);
dCreatePlane(space,0,0,1,0); // Add a ground plane.
contactgroup = dJointGroupCreate (0);
dWorldSetGravity(world,0,0,-9.8);
dWorldSetQuickStepNumIterations(world, 32);
dWorldSetContactMaxCorrectingVel(world, 40);
dWorldSetMaxAngularSpeed(world, 62.8);
dWorldSetERP(world, 0.7);
dWorldSetQuickStepW(world, 0.75); // For increased stability.
dWorldSetAutoDisableFlag( world, true );
dWorldSetAutoDisableLinearThreshold( world, 0.01 );
dWorldSetAutoDisableAngularThreshold( world, 0.03 );
dWorldSetAutoDisableTime( world, 0.15f );
const float kernelrad = 0.7;
mbody = dBodyCreate(world);
dBodySetPosition(mbody, 0,0,0+H);
dMassSetSphere( &m, 5, kernelrad );
dBodySetMass( mbody, &m );
for (int i=0; i<halton_numc; ++i )
{
dGeomID geom = dCreateConvex
(
space,
halton_planes[i],
halton_numf[i],
halton_verts[i],
halton_numv[i],
halton_faces[i]
);
hgeom[i] = geom;
const dReal x = halton_pos[i][0];
const dReal y = halton_pos[i][1];
const dReal z = halton_pos[i][2];
const dReal dsqr = x*x + y*y + z*z;
if ( dsqr < kernelrad*kernelrad && solidkernel )
{
dGeomSetBody(geom, mbody);
dGeomSetOffsetPosition(geom, x,y,z);
}
else
{
dBodyID body = dBodyCreate(world);
hbody[i] = body;
dBodySetPosition(body, x,y,z+H);
dReal volu = halton_volu[i];
dReal rad = pow( volu * 3 / (4*M_PI), (1/3.0) );
dMassSetSphere( &m,5,rad );
dBodySetMass( body,&m );
#if 1
dBodySetLinearDamping (body, 0.0005);
dBodySetAngularDamping(body, 0.0300);
#endif
dGeomSetBody(geom,body);
}
}
// run simulation
const int w=1280;
const int h=720;
dsSimulationLoop (argc,argv,w,h,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

652
thirdparty/ode-0.16.5/ode/demo/demo_crash.cpp vendored Normal file
View File

@@ -0,0 +1,652 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// This is a demo of the QuickStep and StepFast methods,
// originally by David Whittaker.
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define LENGTH 3.5 // chassis length
#define WIDTH 2.5 // chassis width
#define HEIGHT 1.0 // chassis height
#define RADIUS 0.5 // wheel radius
#define STARTZ 1.0 // starting height of chassis
#define CMASS 1 // chassis mass
#define WMASS 1 // wheel mass
#define COMOFFSET -5 // center of mass offset
#define WALLMASS 1 // wall box mass
#define BALLMASS 1 // ball mass
#define FMAX 25 // car engine fmax
#define ROWS 1 // rows of cars
#define COLS 1 // columns of cars
#define ITERS 20 // number of iterations
#define WBOXSIZE 1.0 // size of wall boxes
#define WALLWIDTH 12 // width of wall
#define WALLHEIGHT 10 // height of wall
#define DISABLE_THRESHOLD 0.008 // maximum velocity (squared) a body can have and be disabled
#define DISABLE_STEPS 10 // number of steps a box has to have been disable-able before it will be disabled
#define CANNON_X -10 // x position of cannon
#define CANNON_Y 5 // y position of cannon
#define CANNON_BALL_MASS 10 // mass of the cannon ball
#define CANNON_BALL_RADIUS 0.5
static const dVector3 xunit = { 1, 0, 0 }, yunit = { 0, 1, 0 }, zpunit = { 0, 0, 1 }, zmunit = { 0, 0, -1 };
//#define BOX
#define CARS
#define WALL
//#define BALLS
//#define BALLSTACK
//#define ONEBALL
//#define CENTIPEDE
#define CANNON
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dThreadingImplementationID threading;
static dThreadingThreadPoolID pool;
static dBodyID body[10000];
static int bodies;
static dJointID joint[100000];
static int joints;
static dJointGroupID contactgroup;
static dGeomID ground;
static dGeomID box[10000];
static int boxes;
static dGeomID sphere[10000];
static int spheres;
static dGeomID wall_boxes[10000];
static dBodyID wall_bodies[10000];
static dGeomID cannon_ball_geom;
static dBodyID cannon_ball_body;
static int wb_stepsdis[10000];
static int wb;
static bool doFast;
static dBodyID b;
static dMass m;
// things that the user controls
static dReal turn = 0, speed = 0; // user commands
static dReal cannon_angle=0,cannon_elevation=-1.2;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i,n;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected(b1, b2))
return;
const int N = 4;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0) {
for (i=0; i<n; i++) {
contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;
if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass)
contact[i].surface.mu = 20;
else
contact[i].surface.mu = 0.5;
contact[i].surface.slip1 = 0.0;
contact[i].surface.slip2 = 0.0;
contact[i].surface.soft_erp = 0.8;
contact[i].surface.soft_cfm = 0.01;
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2));
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {3.8548f,9.0843f,7.5900f};
static float hpr[3] = {-145.5f,-22.5f,0.25f};
dsSetViewpoint (xyz,hpr);
printf ("Press:\t'a' to increase speed.\n"
"\t'z' to decrease speed.\n"
"\t',' to steer left.\n"
"\t'.' to steer right.\n"
"\t' ' to reset speed and steering.\n"
"\t'[' to turn the cannon left.\n"
"\t']' to turn the cannon right.\n"
"\t'1' to raise the cannon.\n"
"\t'2' to lower the cannon.\n"
"\t'x' to shoot from the cannon.\n"
"\t'f' to toggle fast step mode.\n"
"\t'r' to reset simulation.\n");
}
void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI)
{
int i;
dMass m;
// chassis body
body[bodyI] = dBodyCreate (world);
dBodySetPosition (body[bodyI],x,y,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (body[bodyI],&m);
box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxI],body[bodyI]);
// wheel bodies
for (i=1; i<=4; i++) {
body[bodyI+i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[bodyI+i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[bodyI+i],&m);
sphere[sphereI+i-1] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]);
}
dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front and back wheel hinges
for (i=0; i<4; i++) {
joint[jointI+i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]);
const dReal *a = dBodyGetPosition (body[bodyI+i+1]);
dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]);
dJointSetHinge2Axes (joint[jointI+i], (i<2 ? zpunit : zmunit), yunit);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[jointI+i],dParamVel2,0);
dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX);
}
//center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint)
dBodyID b = dBodyCreate (world);
dBodySetPosition (b,x,y,STARTZ+COMOFFSET);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (b,&m);
dJointID j = dJointCreateFixed(world, 0);
dJointAttach(j, body[bodyI], b);
dJointSetFixed(j);
//box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT);
//dGeomSetBody (box[boxI+1],b);
bodyI += 5;
jointI += 4;
boxI += 1;
sphereI += 4;
}
static
void shutdownSimulation()
{
// destroy world if it exists
if (bodies)
{
dThreadingImplementationShutdownProcessing(threading);
dThreadingFreeThreadPool(pool);
dWorldSetStepThreadingImplementation(world, NULL, NULL);
dThreadingFreeImplementation(threading);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
bodies = 0;
}
}
static
void setupSimulation()
{
int i;
for (i = 0; i < 1000; i++)
wb_stepsdis[i] = 0;
// recreate world
world = dWorldCreate();
// space = dHashSpaceCreate( 0 );
// space = dSimpleSpaceCreate( 0 );
space = dSweepAndPruneSpaceCreate( 0, dSAP_AXES_XYZ );
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-1.5);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
dWorldSetQuickStepNumIterations (world,ITERS);
threading = dThreadingAllocateMultiThreadedImplementation();
pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
// dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
ground = dCreatePlane (space,0,0,1,0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
wb = 0;
#ifdef CARS
for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
makeCar(x, y, bodies, joints, boxes, spheres);
#endif
#ifdef WALL
bool offset = false;
for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
{
offset = !offset;
for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
{
wall_bodies[wb] = dBodyCreate (world);
dBodySetPosition (wall_bodies[wb],-20,y,z);
dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dMassAdjust (&m, WALLMASS);
dBodySetMass (wall_bodies[wb],&m);
wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
//dBodyDisable(wall_bodies[wb++]);
wb++;
}
}
dMessage(0,"wall boxes: %i", wb);
#endif
#ifdef BALLS
for (dReal x = -7; x <= -4; x+=1)
for (dReal y = -1.5; y <= 1.5; y+=1)
for (dReal z = 1; z <= 4; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, BALLMASS);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef ONEBALL
b = dBodyCreate (world);
dBodySetPosition (b,0,0,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
#endif
#ifdef BALLSTACK
for (dReal z = 1; z <= 6; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,0,0,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 0.1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef CENTIPEDE
dBodyID lastb = 0;
for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
{
// chassis body
b = body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],-15,y,STARTZ);
dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
for (dReal x = -17; x > -20; x-=RADIUS*2)
{
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *a = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
dJointSetHinge2Axes (joint[joints], zpunit, xunit);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *b = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
dJointSetHinge2Axes (joint[joints], zpunit, xunit);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
}
if (lastb)
{
dJointID j = dJointCreateFixed(world,0);
dJointAttach (j, b, lastb);
dJointSetFixed(j);
}
lastb = b;
}
#endif
#ifdef BOX
body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],0,0,HEIGHT/2);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m, 1);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
#endif
#ifdef CANNON
cannon_ball_body = dBodyCreate (world);
cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
dBodySetMass (cannon_ball_body,&m);
dGeomSetBody (cannon_ball_geom,cannon_ball_body);
dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
#endif
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd) {
case 'a': case 'A':
speed += 0.3;
break;
case 'z': case 'Z':
speed -= 0.3;
break;
case ',':
turn += 0.1;
if (turn > 0.3)
turn = 0.3;
break;
case '.':
turn -= 0.1;
if (turn < -0.3)
turn = -0.3;
break;
case ' ':
speed = 0;
turn = 0;
break;
case 'f': case 'F':
doFast = !doFast;
break;
case 'r': case 'R':
shutdownSimulation();
setupSimulation();
break;
case '[':
cannon_angle += 0.1;
break;
case ']':
cannon_angle -= 0.1;
break;
case '1':
cannon_elevation += 0.1;
break;
case '2':
cannon_elevation -= 0.1;
break;
case 'x': case 'X': {
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2];
dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]);
dReal force = 10;
dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]);
dBodySetAngularVel (cannon_ball_body,0,0,0);
break;
}
}
}
// simulation loop
static void simLoop (int pause)
{
int i, j;
dsSetTexture (DS_WOOD);
if (!pause) {
#ifdef BOX
dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
for (j = 0; j < joints; j++)
{
dReal curturn = dJointGetHinge2Angle1 (joint[j]);
//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
dJointSetHinge2Param(joint[j],dParamVel2,speed);
dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
dBodyEnable(dJointGetBody(joint[j],0));
dBodyEnable(dJointGetBody(joint[j],1));
}
if (doFast)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
else
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
{
bool disable = true;
const dReal *lvel = dBodyGetLinearVel(b);
dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
if (lspeed > DISABLE_THRESHOLD)
disable = false;
const dReal *avel = dBodyGetAngularVel(b);
dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
if (aspeed > DISABLE_THRESHOLD)
disable = false;
if (disable)
wb_stepsdis[i]++;
else
wb_stepsdis[i] = 0;
if (wb_stepsdis[i] > DISABLE_STEPS)
{
dBodyDisable(b);
dsSetColor(0.5,0.5,1);
}
else
dsSetColor(1,1,1);
}
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
else
{
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
dsSetColor(1,1,1);
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
dsSetColor (0,1,1);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
for (i = 0; i < boxes; i++)
dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
dsSetColor (1,1,1);
for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
dGeomGetRotation(sphere[i]),RADIUS);
// draw the cannon
dsSetColor (1,1,0);
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
dReal csides[3] = {2,2,2};
dsDrawBox (cpos,R2,csides);
for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
dsDrawCylinder (cpos,R4,3,0.5);
// draw the cannon ball
dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
CANNON_BALL_RADIUS);
}
int main (int argc, char **argv)
{
doFast = true;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dInitODE2(0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
setupSimulation();
dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(8, 0, dAllocateFlagBasicData, NULL);
dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
// dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dThreadingImplementationShutdownProcessing(threading);
dThreadingFreeThreadPool(pool);
dWorldSetStepThreadingImplementation(world, NULL, NULL);
dThreadingFreeImplementation(threading);
shutdownSimulation();
dCloseODE();
return 0;
}

321
thirdparty/ode-0.16.5/ode/demo/demo_cyl.cpp vendored Normal file
View File

@@ -0,0 +1,321 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// Test for non-capped cylinder, by Bram Stolk
#include <ode/odeconfig.h>
#include <assert.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#include "world_geom3.h" // this is our world mesh
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
#define BOX
#define CYL
// some constants
#define RADIUS 0.22 // wheel radius
#define WMASS 0.2 // wheel mass
#define WHEELW 0.2 // wheel width
#define BOXSZ 0.4 // box size
//#define CYL_GEOM_OFFSET // rotate cylinder using geom offset
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
#ifdef BOX
static dBodyID boxbody;
static dGeomID boxgeom;
#endif
#ifdef CYL
static dBodyID cylbody;
static dGeomID cylgeom;
#endif
static dJointGroupID contactgroup;
static dGeomID world_mesh;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
// fprintf(stderr,"testing geoms %p %p\n", o1, o2);
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.slip1 = 0.7;
contact[i].surface.slip2 = 0.7;
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
contact[i].surface.mu = 50.0; // was: dInfinity
contact[i].surface.soft_erp = 0.96;
contact[i].surface.soft_cfm = 0.04;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {-8,-9,3};
static float hpr[3] = {45.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
static void reset_state(void)
{
float sx=-4, sy=-4, sz=2;
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#ifdef BOX
dBodySetPosition (boxbody, sx, sy+1, sz);
dBodySetLinearVel (boxbody, 0,0,0);
dBodySetAngularVel (boxbody, 0,0,0);
dBodySetQuaternion (boxbody, q);
#endif
#ifdef CYL
dBodySetPosition (cylbody, sx, sy, sz);
dBodySetLinearVel (cylbody, 0,0,0);
dBodySetAngularVel (cylbody, 0,0,0);
dBodySetQuaternion (cylbody, q);
#endif
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd)
{
case ' ':
reset_state();
break;
}
}
// simulation loop
static void simLoop (int pause)
{
double simstep = 0.005; // 5ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf(dt/simstep);
for (int i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,1,1);
#ifdef BOX
const dReal *BPos = dBodyGetPosition(boxbody);
const dReal *BRot = dBodyGetRotation(boxbody);
float bpos[3] = {BPos[0], BPos[1], BPos[2]};
float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] };
float sides[3] = {BOXSZ, BOXSZ, BOXSZ};
dsDrawBox
(
bpos,
brot,
sides
); // single precision
#endif
#ifdef CYL
const dReal *CPos = dGeomGetPosition(cylgeom);
const dReal *CRot = dGeomGetRotation(cylgeom);
float cpos[3] = {CPos[0], CPos[1], CPos[2]};
float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
dsDrawCylinder
(
// dBodyGetPosition(cylbody),
// dBodyGetRotation(cylbody),
cpos,
crot,
WHEELW,
RADIUS
); // single precision
#endif
// draw world trimesh
dsSetColor(0.7,0.7,0.4);
dsSetTexture (DS_NONE);
const dReal* Pos = dGeomGetPosition(world_mesh);
float pos[3] = { Pos[0], Pos[1], Pos[2] };
const dReal* Rot = dGeomGetRotation(world_mesh);
float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };
int numi = sizeof(world_indices) / sizeof(dTriIndex);
for (int i=0; i<numi/3; i++)
{
int i0 = world_indices[i*3+0];
int i1 = world_indices[i*3+1];
int i2 = world_indices[i*3+2];
float *v0 = world_vertices+i0*3;
float *v1 = world_vertices+i1*3;
float *v2 = world_vertices+i2*3;
dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
}
}
int main (int argc, char **argv)
{
dMass m;
dMatrix3 R;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 12);
// Create a static world using a triangle mesh that we can collide with.
int numv = sizeof(world_vertices)/(3*sizeof(float));
int numi = sizeof(world_indices)/ sizeof(dTriIndex);
printf("numv=%d, numi=%d\n", numv, numi);
dTriMeshDataID Data = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle
(
Data,
world_vertices,
3 * sizeof(float),
numv,
world_indices,
numi,
3 * sizeof(dTriIndex)
);
world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
dGeomSetPosition(world_mesh, 0, 0, 0.5);
dRFromAxisAndAngle (R, 0,1,0, 0.0);
dGeomSetRotation (world_mesh, R);
#ifdef BOX
boxbody = dBodyCreate (world);
dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ);
dMassAdjust (&m, 1);
dBodySetMass (boxbody,&m);
boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ);
dGeomSetBody (boxgeom,boxbody);
dSpaceAdd (space, boxgeom);
#endif
#ifdef CYL
cylbody = dBodyCreate (world);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (cylbody,&m);
cylgeom = dCreateCylinder(0, RADIUS, WHEELW);
dGeomSetBody (cylgeom,cylbody);
#if defined(CYL_GEOM_OFFSET)
dMatrix3 mat;
dRFromAxisAndAngle(mat,1.0f,0.0f,0.0f,M_PI/2.0);
dGeomSetOffsetRotation(cylgeom,mat);
#endif
dSpaceAdd (space, cylgeom);
#endif
reset_state();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy geoms, then space, then the world.
#ifdef CYL
dGeomDestroy (cylgeom);
#endif
#ifdef BOX
dGeomDestroy (boxgeom);
#endif
dGeomDestroy (world_mesh);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
(void)world_normals; // get rid of compiler warnings
}

240
thirdparty/ode-0.16.5/ode/demo/demo_cylvssphere.cpp vendored Normal file
View File

@@ -0,0 +1,240 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// Test for cylinder vs sphere, by Bram Stolk
#include <ode/odeconfig.h>
#include <assert.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID cylbody;
static dGeomID cylgeom;
static dBodyID sphbody;
static dGeomID sphgeom;
static dJointGroupID contactgroup;
static bool show_contacts = true;
#define CYLRADIUS 0.6
#define CYLLENGTH 2.0
#define SPHERERADIUS 0.5
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawLine dsDrawLineD
#endif
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = 0;
contact[i].surface.mu = 50.0; // was: dInfinity
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
if (show_contacts)
{
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.12,0.12,0.12};
dsSetColorAlpha (0,0,1,0.5);
dsDrawBox (contact[i].geom.pos,RI,ss);
dReal *pos = contact[i].geom.pos;
dReal depth = contact[i].geom.depth;
dReal *norm = contact[i].geom.normal;
dReal endp[3] = {pos[0]+depth*norm[0], pos[1]+depth*norm[1], pos[2]+depth*norm[2]};
dsSetColorAlpha (1,1,1,1);
dsDrawLine (contact[i].geom.pos, endp);
}
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {-8,-9,3};
static float hpr[3] = {45.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd)
{
case ' ':
break;
}
}
// simulation loop
static void simLoop (int pause)
{
dSpaceCollide (space,0,&nearCallback);
if (!pause)
{
dWorldQuickStep (world, 0.01); // 100 Hz
}
dJointGroupEmpty (contactgroup);
dsSetColorAlpha (1,1,0,0.5);
const dReal *CPos = dBodyGetPosition(cylbody);
const dReal *CRot = dBodyGetRotation(cylbody);
float cpos[3] = {CPos[0], CPos[1], CPos[2]};
float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
dsDrawCylinder
(
cpos,
crot,
CYLLENGTH,
CYLRADIUS
); // single precision
const dReal *SPos = dBodyGetPosition(sphbody);
const dReal *SRot = dBodyGetRotation(sphbody);
float spos[3] = {SPos[0], SPos[1], SPos[2]};
float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
dsDrawSphere
(
spos,
srot,
SPHERERADIUS
); // single precision
}
int main (int argc, char **argv)
{
dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 32);
dCreatePlane (space,0,0,1, 0.0);
cylbody = dBodyCreate (world);
dQuaternion q;
#if 0
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#else
// dQFromAxisAndAngle (q,1,0,0, M_PI * 1.0);
dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77);
#endif
dBodySetQuaternion (cylbody,q);
dMassSetCylinder (&m,1.0,3,CYLRADIUS,CYLLENGTH);
dBodySetMass (cylbody,&m);
cylgeom = dCreateCylinder(0, CYLRADIUS, CYLLENGTH);
dGeomSetBody (cylgeom,cylbody);
dBodySetPosition (cylbody, 0, 0, 3);
dSpaceAdd (space, cylgeom);
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,SPHERERADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, SPHERERADIUS);
dGeomSetBody (sphgeom,sphbody);
dBodySetPosition (sphbody, 0, 0, 5.5);
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dGeomDestroy(sphgeom);
dGeomDestroy (cylgeom);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

194
thirdparty/ode-0.16.5/ode/demo/demo_dball.cpp vendored Normal file
View File

@@ -0,0 +1,194 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#define dsDrawBox dsDrawBoxD
#define dsDrawLine dsDrawLineD
#endif
dWorldID world;
dSpaceID space;
dBodyID body1;
dBodyID body2;
dJointID joint1, joint2;
void start()
{
world = dWorldCreate();
dWorldSetGravity (world,0,0,-9.8);
dWorldSetDamping(world, 1e-4, 1e-5);
// dWorldSetERP(world, 1);
space = dSimpleSpaceCreate (0);
body1 = dBodyCreate(world);
body2 = dBodyCreate(world);
dBodySetPosition(body1, 0, 0, 3);
dBodySetPosition(body2, 0, 0, 1);
dGeomID g;
dMass mass;
g = dCreateBox(space, 0.2, 0.2, 1);
dGeomSetBody(g, body1);
dMassSetBox(&mass, 1, 0.2, 0.2, 1);
dBodySetMass(body1, &mass);
g = dCreateBox(space, 0.2, 0.2, 1);
dGeomSetBody(g, body2);
dMassSetBox(&mass, 1, 0.2, 0.2, 1);
dBodySetMass(body2, &mass);
joint1 = dJointCreateDBall(world, 0);
dJointAttach(joint1, body1, 0);
dJointSetDBallAnchor1(joint1, 0, 0, 3.5);
dJointSetDBallAnchor2(joint1, 0, 0, 4.5);
joint2 = dJointCreateDBall(world, 0);
dJointAttach(joint2, body1, body2);
dJointSetDBallAnchor1(joint2, 0, 0, 2.5);
dJointSetDBallAnchor2(joint2, 0, 0, 1.5);
// initial camera position
static float xyz[3] = {3.8966, -2.0614, 4.0300};
static float hpr[3] = {153.5, -16.5, 0};
dsSetViewpoint (xyz,hpr);
}
void stop()
{
dSpaceDestroy(space);
dWorldDestroy(world);
}
void drawGeom(dGeomID g)
{
int gclass = dGeomGetClass(g);
const dReal *pos = dGeomGetPosition(g);
const dReal *rot = dGeomGetRotation(g);
switch (gclass) {
case dSphereClass:
dsSetColorAlpha(0, 0.75, 0.5, 1);
dsSetTexture (DS_CHECKERED);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(g));
break;
case dBoxClass:
{
dVector3 lengths;
dsSetColorAlpha(1, 1, 0, 1);
dsSetTexture (DS_WOOD);
dGeomBoxGetLengths(g, lengths);
dsDrawBox(pos, rot, lengths);
break;
}
default:
{}
}
}
void simLoop(int pause)
{
if (!pause) {
static dReal t = 0;
const dReal step = 0.005;
const unsigned nsteps = 4;
for (unsigned i=0; i<nsteps; ++i) {
dReal f = sin(t*1.2)*0.8;
dBodyAddForceAtRelPos(body1,
f, 0, 0,
0, 0, -0.5); // at the lower end
dReal g = sin(t*0.7)*0.8;
dBodyAddForceAtRelPos(body2,
0, g, 0,
0, 0, -0.5); // at the lower end
t += step;
dWorldQuickStep(world, step);
}
}
// now we draw everything
unsigned ngeoms = dSpaceGetNumGeoms(space);
for (unsigned i=0; i<ngeoms; ++i) {
dGeomID g = dSpaceGetGeom(space, i);
drawGeom(g);
}
dVector3 a11, a12;
dJointGetDBallAnchor1(joint1, a11);
dJointGetDBallAnchor2(joint1, a12);
dsSetColor(1, 0, 0);
dsDrawLine(a11, a12);
//printf("Error 1: %f\n", fabs(dJointGetDBallDistance(joint1) - dCalcPointsDistance3(a11, a12)));
dVector3 a21, a22;
dJointGetDBallAnchor1(joint2, a21);
dJointGetDBallAnchor2(joint2, a22);
dsSetColor(0, 1, 0);
dsDrawLine(a21, a22);
//printf("Error 2: %f\n", fabs(dJointGetDBallDistance(joint2) - dCalcPointsDistance3(a21, a22)));
}
int main(int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = stop;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE();
// run demo
dsSimulationLoop (argc, argv, 800, 600, &fn);
dCloseODE();
return 0;
}

217
thirdparty/ode-0.16.5/ode/demo/demo_dhinge.cpp vendored Normal file
View File

@@ -0,0 +1,217 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#define dsDrawBox dsDrawBoxD
#define dsDrawLine dsDrawLineD
#endif
dWorldID world;
dSpaceID space;
dBodyID body1;
dBodyID body2;
dJointID joint1, joint2;
bool applyForce = false;
void start()
{
world = dWorldCreate();
dWorldSetGravity (world,0,0,-9.8);
dWorldSetDamping(world, 1e-4, 1e-5);
// dWorldSetERP(world, 1);
space = dSimpleSpaceCreate (0);
body1 = dBodyCreate(world);
body2 = dBodyCreate(world);
dBodySetPosition(body1, 0, 0, 3);
dBodySetPosition(body2, 0, 0, 1);
dGeomID g;
dMass mass;
g = dCreateBox(space, 0.2, 0.2, 1);
dGeomSetBody(g, body1);
dMassSetBox(&mass, 1, 0.2, 0.2, 1);
dBodySetMass(body1, &mass);
g = dCreateBox(space, 0.2, 0.2, 1);
dGeomSetBody(g, body2);
dMassSetBox(&mass, 1, 0.2, 0.2, 1);
dBodySetMass(body2, &mass);
#if 1
joint1 = dJointCreateDHinge(world, 0);
dJointAttach(joint1, body1, 0);
dJointSetDHingeAxis(joint1, 0, 1, 0);
dJointSetDHingeAnchor1(joint1, 0, 0, 3.5);
dJointSetDHingeAnchor2(joint1, 0, 0, 4.5);
#endif
#if 1
joint2 = dJointCreateDHinge(world, 0);
dJointAttach(joint2, body1, body2);
dJointSetDHingeAxis(joint2, 1, 0, 0);
dJointSetDHingeAnchor1(joint2, 0, 0, 2.5);
dJointSetDHingeAnchor2(joint2, 0, 0, 1.5);
#else
joint2 = dJointCreateDBall(world, 0);
dJointAttach(joint2, body1, body2);
dJointSetDBallAnchor1(joint2, 0, 0, 2.5);
dJointSetDBallAnchor2(joint2, 0, 0, 1.5);
#endif
//dBodyAddForce(body1, 20, 0, 0);
// initial camera position
static float xyz[3] = {3.8966, -2.0614, 4.0300};
static float hpr[3] = {153.5, -16.5, 0};
dsSetViewpoint (xyz,hpr);
}
void stop()
{
dSpaceDestroy(space);
dWorldDestroy(world);
}
void drawGeom(dGeomID g)
{
int gclass = dGeomGetClass(g);
const dReal *pos = dGeomGetPosition(g);
const dReal *rot = dGeomGetRotation(g);
switch (gclass) {
case dBoxClass:
{
dVector3 lengths;
if (applyForce)
dsSetColor(1, .5, 0);
else
dsSetColor(1, 1, 0);
dsSetTexture (DS_WOOD);
dGeomBoxGetLengths(g, lengths);
dsDrawBox(pos, rot, lengths);
break;
}
default:
{}
}
}
void simLoop(int pause)
{
if (!pause) {
static dReal t = 0;
const dReal step = 0.005;
const unsigned nsteps = 2;
for (unsigned i=0; i<nsteps; ++i) {
applyForce = fmodf(t, 3.) > 2.;
if (applyForce) {
dReal f = 0.3 * sin(t*1.2);
dBodyAddForceAtRelPos(body1,
f, 0, 0,
0, 0, -0.5); // at the lower end
dReal g = 0.3 * sin(t*0.7);
dBodyAddForceAtRelPos(body2,
0, g, 0,
0, 0, -0.5); // at the lower end
}
t += step;
if (t > 20.)
t = 0.;
dWorldQuickStep(world, step);
}
}
// now we draw everything
unsigned ngeoms = dSpaceGetNumGeoms(space);
for (unsigned i=0; i<ngeoms; ++i) {
dGeomID g = dSpaceGetGeom(space, i);
drawGeom(g);
}
#if 1
dVector3 a11, a12;
dJointGetDHingeAnchor1(joint1, a11);
dJointGetDHingeAnchor2(joint1, a12);
dsSetColor(1, 0, 0);
dsDrawLine(a11, a12);
//printf("Error 1: %f\n", fabs(dJointGetDHingeDistance(joint1) - dCalcPointsDistance3(a11, a12)));
#endif
#if 1
dVector3 a21, a22;
dJointGetDHingeAnchor1(joint2, a21);
dJointGetDHingeAnchor2(joint2, a22);
dsSetColor(0, 1, 0);
dsDrawLine(a21, a22);
//printf("Error 2: %f\n", fabs(dJointGetDHingeDistance(joint2) - dCalcPointsDistance3(a21, a22)));
#endif
}
int main(int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = stop;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE();
// run demo
dsSimulationLoop (argc, argv, 800, 600, &fn);
dCloseODE();
return 0;
}

312
thirdparty/ode-0.16.5/ode/demo/demo_feedback.cpp vendored Normal file
View File

@@ -0,0 +1,312 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// Test for breaking joints, by Bram Stolk
#include <ode/odeconfig.h>
#include <assert.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawCylinder dsDrawCylinderD
#endif
// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static const int STACKCNT=10; // nr of weights on bridge
static const int SEGMCNT=16; // nr of segments in bridge
static const float SEGMDIM[3] = { 0.9, 4, 0.1 };
static dGeomID groundgeom;
static dBodyID segbodies[SEGMCNT];
static dGeomID seggeoms[SEGMCNT];
static dBodyID stackbodies[STACKCNT];
static dGeomID stackgeoms[STACKCNT];
static dJointID hinges[SEGMCNT-1];
static dJointID sliders[2];
static dJointFeedback jfeedbacks[SEGMCNT-1];
static dReal colours[SEGMCNT];
static int stress[SEGMCNT-1];
static dJointGroupID contactgroup;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = 100.0;
contact[i].surface.soft_erp = 0.96;
contact[i].surface.soft_cfm = 0.02;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = { -6, 8, 6};
static float hpr[3] = { -65.0f, -27.0f, 0.0f};
dsSetViewpoint (xyz,hpr);
}
// called when a key pressed
static void command (int)
{}
void drawGeom (dGeomID g)
{
const dReal *pos = dGeomGetPosition(g);
const dReal *R = dGeomGetRotation(g);
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
dVector3 sides;
dGeomBoxGetLengths (g, sides);
dsDrawBox (pos,R,sides);
}
if (type == dCylinderClass)
{
dReal r,l;
dGeomCylinderGetParams(g, &r, &l);
dsDrawCylinder (pos, R, l, r);
}
}
static void inspectJoints(void)
{
const dReal forcelimit = 4000.0;
int i;
for (i=0; i<SEGMCNT-1; i++)
{
if (dJointGetBody(hinges[i], 0))
{
// This joint has not snapped already... inspect it.
dReal l0 = dCalcVectorLength3(jfeedbacks[i].f1);
dReal l1 = dCalcVectorLength3(jfeedbacks[i].f2);
colours[i+0] = 0.95*colours[i+0] + 0.05 * l0/forcelimit;
colours[i+1] = 0.95*colours[i+1] + 0.05 * l1/forcelimit;
if (l0 > forcelimit || l1 > forcelimit)
stress[i]++;
else
stress[i]=0;
if (stress[i]>4)
{
// Low-pass filter the noisy feedback data.
// Only after 4 consecutive timesteps with excessive load, snap.
fprintf(stderr,"SNAP! (that was the sound of joint %d breaking)\n", i);
dJointAttach (hinges[i], 0, 0);
}
}
}
}
// simulation loop
static void simLoop (int pause)
{
int i;
double simstep = 0.002; // 2ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf(dt/simstep);
for (i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world, simstep);
dJointGroupEmpty (contactgroup);
inspectJoints();
}
for (i=0; i<SEGMCNT; i++)
{
float r=0,g=0,b=0.2;
float v = colours[i];
if (v>1.0) v=1.0;
if (v<0.5)
{
r=2*v;
g=1.0;
}
else
{
r=1.0;
g=2*(1.0-v);
}
dsSetColor (r,g,b);
drawGeom(seggeoms[i]);
}
dsSetColor (1,1,1);
for (i=0; i<STACKCNT; i++)
drawGeom(stackgeoms[i]);
}
int main (int argc, char **argv)
{
dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 20);
int i;
for (i=0; i<SEGMCNT; i++)
{
segbodies[i] = dBodyCreate (world);
dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dBodySetMass (segbodies[i], &m);
seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
dGeomSetBody (seggeoms[i], segbodies[i]);
dSpaceAdd (space, seggeoms[i]);
}
for (i=0; i<SEGMCNT-1; i++)
{
hinges[i] = dJointCreateHinge (world,0);
dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
dJointSetHingeAxis (hinges[i], 0,1,0);
dJointSetHingeParam (hinges[i],dParamFMax, 8000.0);
// NOTE:
// Here we tell ODE where to put the feedback on the forces for this hinge
dJointSetFeedback (hinges[i], jfeedbacks+i);
stress[i]=0;
}
for (i=0; i<STACKCNT; i++)
{
stackbodies[i] = dBodyCreate(world);
dMassSetBox (&m, 2.0, 2, 2, 0.6);
dBodySetMass(stackbodies[i],&m);
stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
dGeomSetBody(stackgeoms[i], stackbodies[i]);
dBodySetPosition(stackbodies[i], 0,0,8+2*i);
dSpaceAdd(space, stackgeoms[i]);
}
sliders[0] = dJointCreateSlider (world,0);
dJointAttach(sliders[0], segbodies[0], 0);
dJointSetSliderAxis (sliders[0], 1,0,0);
dJointSetSliderParam (sliders[0],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[0],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[0],dParamHiStop, 0.2);
sliders[1] = dJointCreateSlider (world,0);
dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
dJointSetSliderAxis (sliders[1], 1,0,0);
dJointSetSliderParam (sliders[1],dParamFMax, 4000.0);
dJointSetSliderParam (sliders[1],dParamLoStop, 0.0);
dJointSetSliderParam (sliders[1],dParamHiStop, -0.2);
groundgeom = dCreatePlane(space, 0,0,1,0);
for (i=0; i<SEGMCNT; i++)
colours[i]=0.0;
// run simulation
dsSimulationLoop (argc,argv,1280,720,&fn);
dJointGroupEmpty(contactgroup);
dJointGroupDestroy (contactgroup);
// First destroy seggeoms, then space, then the world.
for (i=0; i<SEGMCNT; i++)
dGeomDestroy (seggeoms[i]);
for (i=0; i<STACKCNT; i++)
dGeomDestroy (stackgeoms[i]);
dSpaceDestroy(space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

205
thirdparty/ode-0.16.5/ode/demo/demo_friction.cpp vendored Normal file
View File

@@ -0,0 +1,205 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
test the Coulomb friction approximation.
a 10x10 array of boxes is made, each of which rests on the ground.
a horizantal force is applied to each box to try and get it to slide.
box[i][j] has a mass (i+1)*MASS and a force (j+1)*FORCE. by the Coloumb
friction model, the box should only slide if the force is greater than MU
times the contact normal force, i.e.
f > MU * body_mass * GRAVITY
(j+1)*FORCE > MU * (i+1)*MASS * GRAVITY
(j+1) > (i+1) * (MU*MASS*GRAVITY/FORCE)
(j+1) > (i+1) * k
this should be independent of the number of contact points, as N contact
points will each have 1/N'th the normal force but the pushing force will
have to overcome N contacts. the constants are chosen so that k=1.
thus you should see a triangle made of half the bodies in the array start to
slide.
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define LENGTH 0.2 // box length & width
#define HEIGHT 0.05 // box height
#define MASS 0.2 // mass of box[i][j] = (i+1) * MASS
#define FORCE 0.05 // force applied to box[i][j] = (j+1) * FORCE
#define MU 0.5 // the global mu to use
#define GRAVITY 0.5 // the global gravity to use
#define N1 10 // number of different forces to try
#define N2 10 // number of different masses to try
// dynamics and collision objects
static dWorldID world;
static dSpaceID space;
static dBodyID body[N1][N2];
static dJointGroupID contactgroup;
static dGeomID ground;
static dGeomID box[N1][N2];
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i;
// only collide things with the ground
int g1 = (o1 == ground);
int g2 = (o2 == ground);
if (!(g1 ^ g2)) return;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
dContact contact[3]; // up to 3 contacts per box
for (i=0; i<3; i++) {
contact[i].surface.mode = dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = MU;
contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {1.7772,-0.7924,2.7600};
static float hpr[3] = {90.0000,-54.0000,0.0000};
dsSetViewpoint (xyz,hpr);
}
// simulation loop
static void simLoop (int pause)
{
int i;
if (!pause) {
// apply forces to all bodies
for (i=0; i<N1; i++) {
for (int j=0; j<N2; j++) {
dBodyAddForce (body[i][j],FORCE*(i+1),0,0);
}
}
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,0,1);
dReal sides[3] = {LENGTH,LENGTH,HEIGHT};
for (i=0; i<N1; i++) {
for (int j=0; j<N2; j++) {
dsDrawBox (dGeomGetPosition(box[i][j]),dGeomGetRotation(box[i][j]),
sides);
}
}
}
int main (int argc, char **argv)
{
int i,j;
dMass m;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-GRAVITY);
ground = dCreatePlane (space,0,0,1,0);
// bodies
for (i=0; i<N1; i++) {
for (j=0; j<N2; j++) {
body[i][j] = dBodyCreate (world);
dMassSetBox (&m,1,LENGTH,LENGTH,HEIGHT);
dMassAdjust (&m,MASS*(j+1));
dBodySetMass (body[i][j],&m);
dBodySetPosition (body[i][j],i*2*LENGTH,j*2*LENGTH,HEIGHT*0.5);
box[i][j] = dCreateBox (space,LENGTH,LENGTH,HEIGHT);
dGeomSetBody (box[i][j],body[i][j]);
}
}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

210
thirdparty/ode-0.16.5/ode/demo/demo_gyro2.cpp vendored Normal file
View File

@@ -0,0 +1,210 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
Angular friction demo:
A bunch of ramps of different pitch.
A bunch of spheres with rolling friction.
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// dynamics and collision objects
static dWorldID world = 0;
static const dReal dt = 1/REAL(60.0); // 60 fps
// Water density if units are meters and kg
static const dReal density = 1000;
// A long skinny thing
static dVector3 sides = {2,.5,.25};
// Initial angular velocity
static dVector3 omega = {5,1,2};
static dVector3 torque = {0,10,0};
static dBodyID noGyroBody;
static dBodyID expGyroBody;
static dBodyID impGyroBody;
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {0,-4.0f,3.0f};
static float hpr[3] = {90.0000,-15.0000,0.0000};
dsSetViewpoint (xyz,hpr);
printf ("Press:\n"
"\t'a' to apply a torque\n"
"\t'r' to reset simulation.\n");
}
/**
Delete the bodies, etc.
*/
static void clear()
{
if (world) dWorldDestroy (world);
world = 0;
}
/**
Cleanup if necessary and rebuild the
world.
*/
static void reset()
{
clear();
// create world
world = dWorldCreate();
// Calculate mass for a box;
dMass boxMass;
dMassSetBox(&boxMass,density,sides[0],sides[1],sides[2]);
noGyroBody = dBodyCreate(world);// Conservation of ang-velocity
expGyroBody = dBodyCreate(world);// Explicit conservation of ang-momentum
impGyroBody = dBodyCreate(world);// Implicit conservation of ang-momentum
dBodySetMass( noGyroBody , &boxMass );
dBodySetMass( expGyroBody, &boxMass );
dBodySetMass( impGyroBody, &boxMass );
// Try to avoid collisions.
dReal sep = dCalcVectorLength3(sides);
dBodySetPosition( noGyroBody , -sep, 0, sep);
dBodySetPosition( expGyroBody, 0, 0, sep);
dBodySetPosition( impGyroBody, sep, 0, sep);
// Set the initial angular velocity
dBodySetAngularVel( noGyroBody , omega[0], omega[1], omega[2]);
dBodySetAngularVel( expGyroBody, omega[0], omega[1], omega[2]);
dBodySetAngularVel( impGyroBody, omega[0], omega[1], omega[2]);
dBodySetGyroscopicMode( noGyroBody, 0);
// We compute this ourselves using the math
// that was in the old stepper.
dBodySetGyroscopicMode(expGyroBody, 0);
// Keep things from crashing by limiting
// the angular speed of the explicit body.
// Note that this isn't necessary for
// the other two bodies.
dBodySetMaxAngularSpeed( expGyroBody, 40 );
}
static void command (int cmd)
{
switch (cmd) {
case 'a': case 'A':
dBodyAddTorque( noGyroBody, torque[0], torque[1], torque[2]);
dBodyAddTorque(expGyroBody, torque[0], torque[1], torque[2]);
dBodyAddTorque(impGyroBody, torque[0], torque[1], torque[2]);
break;
case 'r': case 'R':
reset();
break;
}
}
/**
This is the explicit computation of
gyroscopic forces.
*/
static void expStep(dBodyID body)
{
// Explicit computation
dMatrix3 I,tmp;
dMass m;
dBodyGetMass(body,&m);
const dReal* R = dBodyGetRotation(body);
// compute inertia tensor in global frame
dMultiply2_333 (tmp,m.I,R);
dMultiply0_333 (I,R,tmp);
// compute explicit rotational force
// we treat 'tmp'like a vector, but that's okay.
const dReal* w = dBodyGetAngularVel(body);
dMultiply0_331 (tmp,I,w);
dVector3 tau;
dCalcVectorCross3(tau,tmp,w);
dBodyAddTorque(body,tau[0],tau[1],tau[2]);
}
// simulation loop
static void simLoop (int pause)
{
if (!pause) {
expStep(expGyroBody);
dWorldStep (world,dt);
}
dsSetTexture (DS_WOOD);
dsSetColor(1,0,0);
dsDrawBox(dBodyGetPosition(noGyroBody ),dBodyGetRotation(noGyroBody ),sides);
dsSetColor(1,1,0);
dsDrawBox(dBodyGetPosition(expGyroBody),dBodyGetRotation(expGyroBody),sides);
dsSetColor(0,1,0);
dsDrawBox(dBodyGetPosition(impGyroBody),dBodyGetRotation(impGyroBody),sides);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dInitODE2(0);
reset();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
clear();
dCloseODE();
return 0;
}

258
thirdparty/ode-0.16.5/ode/demo/demo_gyroscopic.cpp vendored Normal file
View File

@@ -0,0 +1,258 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawConvex dsDrawConvexD
#endif
bool write_world = false;
bool show_contacts = false;
dWorld * world;
dBody *top1, *top2;
dSpace *space;
dJointGroup contactgroup;
const dReal pinradius = 0.05f;
const dReal pinlength = 1.5f;
const dReal topradius = 1.0f;
const dReal toplength = 0.25f;
const dReal topmass = 1.0f;
#define MAX_CONTACTS 4
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
// for drawing the contact points
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
int i;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
dContact contact[MAX_CONTACTS];
int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact));
for (i=0; i<numc; i++) {
contact[i].surface.mode = dContactApprox1;
contact[i].surface.mu = 2;
dJointID c = dJointCreateContact (*world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts)
dsDrawBox (contact[i].geom.pos, RI, ss);
}
}
// start simulation - set viewpoint
static void start()
{
static float xyz[3] = {4.777f, -2.084f, 2.18f};
static float hpr[3] = {153.0f, -14.5f, 0.0f};
dsSetViewpoint (xyz,hpr);
printf ("Orange top approximates conservation of angular momentum\n");
printf ("Green top uses conservation of angular velocity\n");
printf ("---\n");
printf ("SPACE to reset\n");
printf ("A to tilt the tops.\n");
printf ("T to toggle showing the contact points.\n");
printf ("1 to save the current state to 'state.dif'.\n");
}
char locase (char c)
{
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void reset();
static void tilt();
static void command (int cmd)
{
cmd = locase (cmd);
if (cmd == ' ')
{
reset();
}
else if (cmd == 'a') {
tilt();
}
else if (cmd == 't') {
show_contacts = !show_contacts;
}
else if (cmd == '1') {
write_world = true;
}
}
// simulation loop
static void simLoop (int pause)
{
dsSetColor (0,0,2);
space->collide(0,&nearCallback);
if (!pause)
//world->quickStep(0.02);
world->step(0.02);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (*world,f,"X");
fclose (f);
}
write_world = false;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetTexture (DS_WOOD);
dsSetColor (1,0.5f,0);
dsDrawCylinder(top1->getPosition(),
top1->getRotation(),
toplength, topradius);
dsDrawCapsule(top1->getPosition(),
top1->getRotation(),
pinlength, pinradius);
dsSetColor (0.5f,1,0);
dsDrawCylinder(top2->getPosition(),
top2->getRotation(),
toplength, topradius);
dsDrawCapsule(top2->getPosition(),
top2->getRotation(),
pinlength, pinradius);
}
static void reset()
{
dMatrix3 R;
dRSetIdentity(R);
top1->setRotation(R);
top2->setRotation(R);
top1->setPosition(0.8f, -2, 2);
top2->setPosition(0.8f, 2, 2);
top1->setAngularVel(1,0,7);
top2->setAngularVel(1,0,7);
top1->setLinearVel(0,0.2f,0);
top2->setLinearVel(0,0.2f,0);
}
static void tilt()
{
top1->addTorque(0, 10, 0);
top2->addTorque(0, 10, 0);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE();
world = new dWorld();
world->setGravity(0,0,-0.5f);
world->setCFM(1e-5f);
world->setLinearDamping(0.00001f);
world->setAngularDamping(0.0001f);
space = new dSimpleSpace(0);
dPlane *floor = new dPlane(*space, 0,0,1,0);
top1 = new dBody(*world);
top2 = new dBody(*world);
dMass m;
m.setCylinderTotal(1, 3, topradius, toplength);
top1->setMass(m);
top2->setMass(m);
dGeom *g1, *g2, *pin1, *pin2;
g1 = new dCylinder(*space, topradius, toplength);
g1->setBody(*top1);
g2 = new dCylinder(*space, topradius, toplength);
g2->setBody(*top2);
pin1 = new dCapsule(*space, pinradius, pinlength);
pin1->setBody(*top1);
pin2 = new dCapsule(*space, pinradius, pinlength);
pin2->setBody(*top2);
top2->setGyroscopicMode(false);
reset();
// run simulation
dsSimulationLoop (argc,argv,512,384,&fn);
delete g1;
delete g2;
delete pin1;
delete pin2;
delete floor;
contactgroup.empty();
delete top1;
delete top2;
delete space;
delete world;
dCloseODE();
}

714
thirdparty/ode-0.16.5/ode/demo/demo_heightfield.cpp vendored Normal file
View File

@@ -0,0 +1,714 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#include "bunny_geom.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
#define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians
int g_allow_trimesh;
// Our heightfield geom
dGeomID gheight;
// Heightfield dimensions
#define HFIELD_WSTEP 15 // Vertex count along edge >= 2
#define HFIELD_DSTEP 31
#define HFIELD_WIDTH REAL( 4.0 )
#define HFIELD_DEPTH REAL( 8.0 )
#define HFIELD_WSAMP ( HFIELD_WIDTH / ( HFIELD_WSTEP-1 ) )
#define HFIELD_DSAMP ( HFIELD_DEPTH / ( HFIELD_DSTEP-1 ) )
//<---- Convex Object
dReal planes[]= // planes for a cube
{
1.0f ,0.0f ,0.0f ,0.25f,
0.0f ,1.0f ,0.0f ,0.25f,
0.0f ,0.0f ,1.0f ,0.25f,
0.0f ,0.0f ,-1.0f,0.25f,
0.0f ,-1.0f,0.0f ,0.25f,
-1.0f,0.0f ,0.0f ,0.25f
/*
1.0f ,0.0f ,0.0f ,2.0f,
0.0f ,1.0f ,0.0f ,1.0f,
0.0f ,0.0f ,1.0f ,1.0f,
0.0f ,0.0f ,-1.0f,1.0f,
0.0f ,-1.0f,0.0f ,1.0f,
-1.0f,0.0f ,0.0f ,0.0f
*/
};
const unsigned int planecount=6;
dReal points[]= // points for a cube
{
0.25f,0.25f,0.25f, // point 0
-0.25f,0.25f,0.25f, // point 1
0.25f,-0.25f,0.25f, // point 2
-0.25f,-0.25f,0.25f,// point 3
0.25f,0.25f,-0.25f, // point 4
-0.25f,0.25f,-0.25f,// point 5
0.25f,-0.25f,-0.25f,// point 6
-0.25f,-0.25f,-0.25f,// point 7
};
const unsigned int pointcount=8;
unsigned int polygons[] = //Polygons for a cube (6 squares)
{
4,0,2,6,4, // positive X
4,1,0,4,5, // positive Y
4,0,1,3,2, // positive Z
4,3,1,5,7, // negative X
4,2,3,7,6, // negative Y
4,5,4,6,7, // negative Z
};
//----> Convex Object
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawConvex dsDrawConvexD
#define dsDrawTriangle dsDrawTriangleD
#endif
// some constants
#define NUM 100 // max number of objects
#define DENSITY (5.0) // density of all objects
#define GPB 3 // maximum number of geometries per body
#define MAX_CONTACTS 64 // maximum number of contact points per body
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom[GPB]; // geometries representing this body
// Trimesh only - double buffered matrices for 'last transform' setup
dReal matrix_dblbuff[ 16 * 2 ];
int last_matrix_index;
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1; // selected object
static int show_aabb = 0; // show geom AABBs?
static int show_contacts = 0; // show contact points?
static int random_pos = 1; // drop objects from random position?
static int write_world = 0;
//============================
dGeomID TriMesh1;
dGeomID TriMesh2;
//static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data
//============================
dReal heightfield_callback( void*, int x, int z )
{
dReal fx = ( ((dReal)x) - ( HFIELD_WSTEP-1 )/2 ) / (dReal)( HFIELD_WSTEP-1 );
dReal fz = ( ((dReal)z) - ( HFIELD_DSTEP-1 )/2 ) / (dReal)( HFIELD_DSTEP-1 );
// Create an interesting 'hump' shape
dReal h = REAL( 1.0 ) + ( REAL( -16.0 ) * ( fx*fx*fx + fz*fz*fz ) );
return h;
}
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact))
return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i<MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide(o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
dMatrix3 RI;
dRSetIdentity(RI);
const dReal ss[3] = {0.02,0.02,0.02};
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact(world,contactgroup,contact+i);
dJointAttach(c,b1,b2);
if (show_contacts) {
dsSetColor(0,0,1);
dsDrawBox(contact[i].geom.pos,RI,ss);
}
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf("To drop another object, press:\n");
printf(" b for box.\n");
printf(" s for sphere.\n");
printf(" c for capsule.\n");
printf(" y for cylinder.\n");
printf(" v for a convex object.\n");
printf(" x for a composite object.\n");
if ( g_allow_trimesh )
printf(" m for a trimesh.\n");
printf("To select an object, press space.\n");
printf("To disable the selected object, press d.\n");
printf("To enable the selected object, press e.\n");
printf("To toggle showing the geom AABBs, press a.\n");
printf("To toggle showing the contact points, press t.\n");
printf("To toggle dropping from random position/orientation, press r.\n");
printf("To save the current state to 'state.dif', press 1.\n");
}
char locase(char c)
{
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void command(int cmd)
{
dsizeint i;
int j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
//
// Geom Creation
//
if ( cmd == 'b' || cmd == 's' || cmd == 'c' || ( cmd == 'm' && g_allow_trimesh ) ||
cmd == 'x' || cmd == 'y' || cmd == 'v' ) {
if ( num < NUM ) {
i = num;
num++;
} else {
i = nextobj++;
nextobj %= num;
// destroy the body and geoms for slot i
dBodyDestroy(obj[i].body);
obj[i].body = 0;
for (k=0; k < GPB; k++)
if (obj[i].geom[k]) {
dGeomDestroy(obj[i].geom[k]);
obj[i].geom[k] = 0;
}
}
obj[i].body = dBodyCreate(world);
for (k=0; k<3; k++)
sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition(obj[i].body,
(dRandReal()-0.5)*HFIELD_WIDTH*0.75,
(dRandReal()-0.5)*HFIELD_DEPTH*0.75,
dRandReal() + 2 );
dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
} else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition(obj[k].body);
if (pos[2] > maxheight)
maxheight = pos[2];
}
dBodySetPosition(obj[i].body, 0,maxheight+1,0);
dRFromAxisAndAngle(R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation(obj[i].body,R);
if (cmd == 'b') {
dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]);
} else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule(space,sides[0],sides[1]);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
} else if (cmd == 'y') {
dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]);
} else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere(&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere(space,sides[0]);
} else if (cmd == 'm' && g_allow_trimesh) {
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
} else if (cmd == 'x') {
setBody = 1;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
//
// Control Commands
//
if (cmd == ' ') {
selected++;
if (selected >= num)
selected = 0;
if (selected < -1)
selected = 0;
} else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable(obj[selected].body);
} else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable(obj[selected].body);
} else if (cmd == 'a') {
show_aabb = !show_aabb;
} else if (cmd == 't') {
show_contacts = !show_contacts;
} else if (cmd == 'r') {
random_pos = !random_pos;
} else if (cmd == '1') {
write_world = 1;
}
}
// draw a geom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g)
return;
if (!pos)
pos = dGeomGetPosition(g);
if (!R)
R = dGeomGetRotation(g);
int type = dGeomGetClass(g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths(g,sides);
dsDrawBox(pos,R,sides);
} else if (type == dSphereClass) {
dsDrawSphere(pos,R,dGeomSphereGetRadius(g));
} else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams(g,&radius,&length);
dsDrawCapsule(pos,R,length,radius);
} else if (type == dConvexClass) {
//dVector3 sides={0.50,0.50,0.50};
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
} else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams(g,&radius,&length);
dsDrawCylinder(pos,R,length,radius);
} else if (type == dTriMeshClass) {
dTriIndex* Indices = (dTriIndex*)::Indices;
// assume all trimeshes are drawn as bunnies
for (int ii = 0; ii < IndexCount / 3; ii++) {
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[ii * 3 + 0] * 3 + 0],
Vertices[Indices[ii * 3 + 0] * 3 + 1],
Vertices[Indices[ii * 3 + 0] * 3 + 2],
Vertices[Indices[ii * 3 + 1] * 3 + 0],
Vertices[Indices[ii * 3 + 1] * 3 + 1],
Vertices[Indices[ii * 3 + 1] * 3 + 2],
Vertices[Indices[ii * 3 + 2] * 3 + 0],
Vertices[Indices[ii * 3 + 2] * 3 + 1],
Vertices[Indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(pos, R, &v[0], &v[3], &v[6], 1);
}
} else if (type == dHeightfieldClass) {
// Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode.
int ox = (int) ( -HFIELD_WIDTH/2 );
int oz = (int) ( -HFIELD_DEPTH/2 );
// for ( int tx = -1; tx < 2; ++tx )
// for ( int tz = -1; tz < 2; ++tz )
dsSetColorAlpha (0.5,1,0.5,0.5);
dsSetTexture( DS_WOOD );
for ( int i = 0; i < HFIELD_WSTEP - 1; ++i )
for ( int j = 0; j < HFIELD_DSTEP - 1; ++j ) {
dReal a[3], b[3], c[3], d[3];
a[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
a[ 1 ] = heightfield_callback( NULL, i, j );
a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
b[ 1 ] = heightfield_callback( NULL, i + 1, j );
b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
c[ 1 ] = heightfield_callback( NULL, i, j + 1 );
c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 );
d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
dsDrawTriangle( pos, R, a, c, b, 1 );
dsDrawTriangle( pos, R, b, c, d, 1 );
}
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB(g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++)
bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int i=0; i<3; i++)
bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity(RI);
dsSetColorAlpha(1,0,0,0.5);
dsDrawBox(bbpos,RI,bbsides);
}
}
// simulation loop
static void simLoop (int pause)
{
int i,j;
dSpaceCollide(space,0,&nearCallback);
if (!pause)
dWorldQuickStep(world,0.05);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF(world,f,"X");
fclose (f);
}
write_world = 0;
}
// remove all contact joints
dJointGroupEmpty(contactgroup);
//
// Draw Heightfield
//
drawGeom(gheight, 0, 0, 0);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (i=0; i<num; i++) {
for (j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
} else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0.8,0);
} else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
int main (int argc, char **argv)
{
printf("ODE configuration: %s\n", dGetConfiguration());
// Is trimesh support built into this ODE?
g_allow_trimesh = dCheckConfiguration( "ODE_EXT_trimesh" );
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity(world,0,0,-0.05);
dWorldSetCFM(world,1e-5);
dWorldSetAutoDisableFlag(world,1);
dWorldSetContactMaxCorrectingVel(world,0.1);
dWorldSetContactSurfaceLayer(world,0.001);
memset(obj,0,sizeof(obj));
dWorldSetAutoDisableAverageSamplesCount( world, 1 );
// base plane to catch overspill
dCreatePlane( space, 0, 0, 1, 0 );
// our heightfield floor
dHeightfieldDataID heightid = dGeomHeightfieldDataCreate();
// Create an finite heightfield.
dGeomHeightfieldDataBuildCallback( heightid, NULL, heightfield_callback,
HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP,
REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 );
// Give some very bounds which, while conservative,
// makes AABB computation more accurate than +/-INF.
dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) );
gheight = dCreateHeightfield( space, heightid, 1 );
dVector3 pos;
pos[ 0 ] = 0;
pos[ 1 ] = 0;
pos[ 2 ] = 0;
// Rotate so Z is up, not Y (which is the default orientation)
dMatrix3 R;
dRSetIdentity( R );
dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 );
// Place it.
dGeomSetRotation( gheight, R );
dGeomSetPosition( gheight, pos[0], pos[1], pos[2] );
dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
// dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dThreadingImplementationShutdownProcessing(threading);
dThreadingFreeThreadPool(pool);
dWorldSetStepThreadingImplementation(world, NULL, NULL);
dThreadingFreeImplementation(threading);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
// destroy heightfield data, because _we_ own it not ODE
dGeomHeightfieldDataDestroy( heightid );
dCloseODE();
}

165
thirdparty/ode-0.16.5/ode/demo/demo_hinge.cpp vendored Normal file
View File

@@ -0,0 +1,165 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#endif
// some constants
#define SIDE (0.5f) // side length of a box
#define MASS (1.0) // mass of a box
// dynamics and collision objects
static dWorldID world;
static dBodyID body[2];
static dJointID hinge;
// state set by keyboard commands
static int occasional_error = 0;
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press 'e' to start/stop occasional error.\n");
}
// called when a key pressed
static void command (int cmd)
{
if (cmd == 'e' || cmd == 'E') {
occasional_error ^= 1;
}
}
// simulation loop
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
dWorldStep (world,0.05);
a += 0.01;
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE,SIDE,SIDE*0.8f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dQuaternion q;
dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
dBodySetQuaternion (body[0],q);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
dBodySetQuaternion (body[1],q);
hinge = dJointCreateHinge (world,0);
dJointAttach (hinge,body[0],body[1]);
dJointSetHingeAnchor (hinge,0,0,1);
dJointSetHingeAxis (hinge,1,-1,1.41421356);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}

434
thirdparty/ode-0.16.5/ode/demo/demo_jointPR.cpp vendored Normal file
View File

@@ -0,0 +1,434 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
This file try to demonstrate how the PR joint is working.
The axisP is draw in red and the axisR is in green
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <iostream>
#include <math.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#endif
// physics parameters
#define BOX1_LENGTH 2 // Size along the X axis
#define BOX1_WIDTH 1 // Size along the Y axis
#define BOX1_HEIGHT 0.4 // Size along the Z axis (up) since gravity is (0,0,-10)
#define BOX2_LENGTH 0.2
#define BOX2_WIDTH 0.1
#define BOX2_HEIGHT 0.4
#define Mass1 10
#define Mass2 0.1
#define PRISMATIC_ONLY 1
#define ROTOIDE_ONLY 2
int flag = 0;
//camera view
static float xyz[3] = {2.0f,-3.5f,2.0000f};
static float hpr[3] = {90.000f,-25.5000f,0.0000f};
//world,space,body & geom
static dWorldID world;
static dSpaceID space;
static dSpaceID box1_space;
static dBodyID box1_body[1];
static dBodyID box2_body[1];
static dJointID joint[1];
static dJointGroupID contactgroup;
static dGeomID ground;
static dGeomID box1[1];
static dGeomID box2[1];
//collision detection
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i,n;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
const int N = 10;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0)
{
for (i=0; i<n; i++)
{
contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = 0.1;
contact[i].surface.slip1 = 0.02;
contact[i].surface.slip2 = 0.02;
contact[i].surface.soft_erp = 0.1;
contact[i].surface.soft_cfm = 0.0001;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2));
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
printf ("Press 'd' to add force along positive x direction.\nPress 'a' to add force along negative x direction.\n");
printf ("Press 'w' to add force along positive y direction.\nPress 's' to add force along negative y direction.\n");
printf ("Press 'e' to add torque around positive z direction.\nPress 'q' to add torque around negative z direction.\n");
printf ("Press 'o' to add force around positive x direction \n");
printf("Press 'v' to give a defined velocity and add a FMax to the rotoide axis\n");
printf("Press 'c' to set the velocity to zero and remove the FMax\n");
printf("Press 'l' to add limits (-0.5 to 0.5rad) on the rotoide axis\n");
printf("Press 'k' to remove the limits on the rotoide axis\n");
printf("Press 'i' to get joint info\n");
}
// function to update camera position at each step.
void update()
{
// const dReal *a =(dBodyGetPosition (box1_body[0]));
// float dx=a[0];
// float dy=a[1];
// float dz=a[2];
// xyz[0]=dx;
// xyz[1]=dy-5;
// xyz[2]=dz+2;
// hpr[1]=-22.5000f;
// dsSetViewpoint (xyz,hpr);
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd)
{
case 'w':
case 'W':
dBodyAddForce(box2_body[0],0,500,0);
std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n';
break;
case 's':
case 'S':
dBodyAddForce(box2_body[0],0,-500,0);
std::cout<<(dBodyGetPosition(box2_body[0])[1]-dBodyGetPosition(box1_body[0])[1])<<'\n';
break;
case 'd':
case 'D':
dBodyAddForce(box2_body[0],500,0,0);
std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n';
break;
case 'a':
case 'A':
dBodyAddForce(box2_body[0],-500,0,0);
std::cout<<(dBodyGetPosition(box2_body[0])[0]-dBodyGetPosition(box1_body[0])[0])<<'\n';
break;
case 'e':
case 'E':
dBodyAddRelTorque(box2_body[0],0,0,200);
break;
case 'q':
case 'Q':
dBodyAddRelTorque(box2_body[0],0,0,-200);
break;
case 'o':
case 'O':
dBodyAddForce(box1_body[0],10000,0,0);
break;
case 'v':
case 'V':
dJointSetPRParam(joint[0], dParamVel2, 2);
dJointSetPRParam(joint[0], dParamFMax2, 500);
break;
case 'c':
case 'C':
dJointSetPRParam(joint[0], dParamVel2, 0);
dJointSetPRParam(joint[0], dParamFMax2, 0);
break;
case 'l':
case 'L':
dJointSetPRParam(joint[0], dParamLoStop2, -0.5);
dJointSetPRParam(joint[0], dParamHiStop2, 0.5);
break;
case 'k':
case 'K':
dJointSetPRParam(joint[0], dParamLoStop2, -dInfinity);
dJointSetPRParam(joint[0], dParamHiStop2, dInfinity);
break;
case 'i':
case 'I':
dVector3 anchor;
dJointGetPRAnchor(joint[0], anchor);
dReal angle = dJointGetPRAngle(joint[0]);
dReal w = dJointGetPRAngleRate(joint[0]);
dReal l = dJointGetPRPosition(joint[0]);
dReal v = dJointGetPRPositionRate(joint[0]);
printf("Anchor: [%6.4f, %6.4f, %6.4f]\n", anchor[0], anchor[1], anchor[2]);
printf("Position: %7.4f, Rate: %7.4f\n", l, v);
printf("Angle: %7.4f, Rate: %7.4f\n", angle, w);
break;
}
}
// simulation loop
static void simLoop (int pause)
{
if (!pause)
{
//draw 2 boxes
dVector3 ss;
dsSetTexture (DS_WOOD);
const dReal *posBox2 = dGeomGetPosition(box2[0]);
const dReal *rotBox2 = dGeomGetRotation(box2[0]);
dsSetColor (1,1,0);
dGeomBoxGetLengths (box2[0],ss);
dsDrawBox (posBox2, rotBox2, ss);
const dReal *posBox1 = dGeomGetPosition(box1[0]);
const dReal *rotBox1 = dGeomGetRotation(box1[0]);
dsSetColor (1,1,2);
dGeomBoxGetLengths (box1[0], ss);
dsDrawBox (posBox1, rotBox1, ss);
dVector3 anchorPos;
dJointGetPRAnchor (joint[0], anchorPos);
// Draw the axisP
if (ROTOIDE_ONLY != flag )
{
dsSetColor (1,0,0);
dVector3 sizeP = {0, 0.1, 0.1};
for (int i=0; i<3; ++i)
sizeP[0] += (anchorPos[i] - posBox1[i])*(anchorPos[i] - posBox1[i]);
sizeP[0] = sqrt(sizeP[0]);
dVector3 posAxisP;
for (int i=0; i<3; ++i)
posAxisP[i] = posBox1[i] + (anchorPos[i] - posBox1[i])/2.0;
dsDrawBox (posAxisP, rotBox1, sizeP);
}
// Draw the axisR
if (PRISMATIC_ONLY != flag )
{
dsSetColor (0,1,0);
dVector3 sizeR = {0, 0.1, 0.1};
for (int i=0; i<3; ++i)
sizeR[0] += (anchorPos[i] - posBox2[i])*(anchorPos[i] - posBox2[i]);
sizeR[0] = sqrt(sizeR[0]);
dVector3 posAxisR;
for (int i=0; i<3; ++i)
posAxisR[i] = posBox2[i] + (anchorPos[i] - posBox2[i])/2.0;
dsDrawBox (posAxisR, rotBox2, sizeR);
}
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world,0.0001);
update();
dJointGroupEmpty (contactgroup);
}
}
void Help(char **argv)
{
printf("%s ", argv[0]);
printf(" -h | --help : print this help\n");
printf(" -b | --both : Display how the complete joint works\n");
printf(" Default behavior\n");
printf(" -p | --prismatic-only : Display how the prismatic part works\n");
printf(" The anchor pts is set at the center of body 2\n");
printf(" -r | --rotoide-only : Display how the rotoide part works\n");
printf(" The anchor pts is set at the center of body 1\n");
printf(" -t | --texture-path path : Path to the texture.\n");
printf(" Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
printf("--------------------------------------------------\n");
printf("Hit any key to continue:");
getchar();
exit(0);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if (argc >= 2 )
{
for (int i=1; i < argc; ++i)
{
if ( 0 == strcmp("-h", argv[i]) || 0 == strcmp("--help", argv[i]) )
Help(argv);
if (!flag && (0 == strcmp("-p", argv[i]) ||0 == strcmp("--prismatic-only", argv[i])) )
flag = PRISMATIC_ONLY;
if (!flag && (0 == strcmp("-r", argv[i]) || 0 == strcmp("--rotoide-only", argv[i])) )
flag = ROTOIDE_ONLY;
if (0 == strcmp("-t", argv[i]) || 0 == strcmp("--texture-path", argv[i]))
{
int j = i+1;
if ( j >= argc || // Check if we have enough arguments
argv[j][0] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help(argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
}
dInitODE2(0);
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-10);
ground = dCreatePlane (space,0,0,1,0);
//create two boxes
dMass m;
box1_body[0] = dBodyCreate (world);
dMassSetBox (&m,1,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT);
dMassAdjust (&m,Mass1);
dBodySetMass (box1_body[0],&m);
box1[0] = dCreateBox (0,BOX1_LENGTH,BOX1_WIDTH,BOX1_HEIGHT);
dGeomSetBody (box1[0],box1_body[0]);
box2_body[0] = dBodyCreate (world);
dMassSetBox (&m,10,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT);
dMassAdjust (&m,Mass2);
dBodySetMass (box2_body[0],&m);
box2[0] = dCreateBox (0,BOX2_LENGTH,BOX2_WIDTH,BOX2_HEIGHT);
dGeomSetBody (box2[0],box2_body[0]);
//set the initial positions of body1 and body2
dMatrix3 R;
dRSetIdentity(R);
dBodySetPosition (box1_body[0],0,0,BOX1_HEIGHT/2.0);
dBodySetRotation (box1_body[0], R);
dBodySetPosition (box2_body[0],
2.1,
0.0,
BOX2_HEIGHT/2.0);
dBodySetRotation (box2_body[0], R);
//set PR joint
joint[0] = dJointCreatePR(world,0);
dJointAttach (joint[0],box1_body[0],box2_body[0]);
switch (flag)
{
case PRISMATIC_ONLY:
dJointSetPRAnchor (joint[0],
2.1,
0.0,
BOX2_HEIGHT/2.0);
dJointSetPRParam (joint[0],dParamLoStop, -0.5);
dJointSetPRParam (joint[0],dParamHiStop, 1.5);
break;
case ROTOIDE_ONLY:
dJointSetPRAnchor (joint[0],
0.0,
0.0,
BOX2_HEIGHT/2.0);
dJointSetPRParam (joint[0],dParamLoStop, 0.0);
dJointSetPRParam (joint[0],dParamHiStop, 0.0);
break;
default:
dJointSetPRAnchor (joint[0],
1.1,
0.0,
BOX2_HEIGHT/2.0);
dJointSetPRParam (joint[0],dParamLoStop, -0.5);
dJointSetPRParam (joint[0],dParamHiStop, 1.5);
break;
}
dJointSetPRAxis1(joint[0],1,0,0);
dJointSetPRAxis2(joint[0],0,0,1);
// We position the 2 body
// The position of the rotoide joint is on the second body so it can rotate on itself
// and move along the X axis.
// With this anchor
// - A force in X will move only the body 2 inside the low and hi limit
// of the prismatic
// - A force in Y will make the 2 bodies to rotate around on the plane
box1_space = dSimpleSpaceCreate (space);
dSpaceSetCleanup (box1_space,0);
dSpaceAdd(box1_space,box1[0]);
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

736
thirdparty/ode-0.16.5/ode/demo/demo_jointPU.cpp vendored Normal file
View File

@@ -0,0 +1,736 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
This program demonstrates how the PU joint works.
A PU joint is a combination of a Universal joint and a Slider joint.
It is a universal joint with a slider between the anchor point and
body 1.
The upper yellow body is fixed to the world
The lower yellow body is attached to the upper body by a PU joint
The green object is one aprt of the slider.
The purple object is the second part of the slider.
The red object represent the axis1 of the universal part.
The blue object represent the axis2 of the universal part.
The gray object represent the anchor2 of the PU joint.
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <iostream>
#include <math.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
enum IDX_CYL_DIM
{
RADIUS,
LENGTH,
NUM_CYL_DIM
};
const dVector3 boxDim = {1,1,1};
const dVector3 extDim = {0.2,0.2,1.2};
const dVector3 ancDim = {0.2,0.2,0.5};
const dReal axDim[NUM_CYL_DIM] = {0.1,1.0};
int type = dJointTypePU;
const dReal VEL_INC = 0.01; // Velocity increment
// physics parameters
const dReal PI = 3.14159265358979323846264338327950288419716939937510;
const dReal INT_EXT_RATIO = 0.8;
#define X 0
#define Y 1
#define Z 2
enum INDEX
{
W = 0,
D,
EXT,
INT,
AXIS1,
AXIS2,
ANCHOR,
GROUND,
NUM_PARTS,
ALL = NUM_PARTS,
// INDEX for catBits
JOINT,
LAST_INDEX_CNT
};
const int catBits[LAST_INDEX_CNT] =
{
0x0001, ///< W Cylinder category
0x0002, ///< D Cylinder category
0x0004, ///< EXT sliderr category
0x0008, ///< INT slider category
0x0010, ///< AXIS1 universal category
0x0020, ///< AXIS2 universal category
0x0040, ///< ANCHOR category
0x0080, ///< Ground category
~0L, ///< All categories
0x0004 | 0x0008 | 0x0010 | 0x0020 ///< JOINT category
};
#define Mass1 10
#define Mass2 8
//camera view
static float xyz[3] = {6.0f,0.0f,6.0000f};
static float hpr[3] = {-180.000f,-25.5000f,0.0000f};
//world,space,body & geom
static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;
static dBodyID body[NUM_PARTS];
static dGeomID geom[NUM_PARTS];
static dJoint *joint;
const dReal BOX_SIDES[3] = {1.0,1.0,1.0};
const dReal OBS_SIDES[3] = {0.4,0.4,0.4};
const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2};
//collision detection
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i,n;
const int N = 10;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) );
if (n > 0) {
for (i=0; i<n; i++) {
contact[i].surface.mode = (dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM |
dContactApprox1);
contact[i].surface.mu = 0.1;
contact[i].surface.slip1 = 0.02;
contact[i].surface.slip2 = 0.02;
contact[i].surface.soft_erp = 0.1;
contact[i].surface.soft_cfm = 0.0001;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) );
}
}
}
static void printKeyBoardShortCut()
{
printf ("Press 'h' for this help.\n");
printf ("Press 'q' to add force on BLUE body along positive x direction.\n");
printf ("Press 'w' to add force on BLUE body along negative x direction.\n");
printf ("Press 'a' to add force on BLUE body along positive y direction.\n");
printf ("Press 's' to add force on BLUE body along negative y direction.\n");
printf ("Press 'z' to add force on BLUE body along positive z direction.\n");
printf ("Press 'x' to add force on BLUE body along negative z direction.\n");
printf ("Press 'e' to add torque on BLUE body around positive x direction \n");
printf ("Press 'r' to add torque on BLUE body around negative x direction \n");
printf ("Press 'd' to add torque on BLUE body around positive y direction \n");
printf ("Press 'f' to add torque on BLUE body around negative y direction \n");
printf ("Press 'c' to add torque on BLUE body around positive z direction \n");
printf ("Press 'v' to add torque on BLUE body around negative z direction \n");
printf ("Press '.' to increase joint velocity along the prismatic direction.\n");
printf ("Press ',' to decrease joint velocity along the prismatic direction.\n");
printf ("Press 'l' Toggle ON/OFF the limits on all the axis\n");
printf ("Press 'g' Toggle ON/OFF the gravity\n");
printf ("Press 'p' to print the position, angle and rates of the joint.\n");
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
printf ("This program demonstrates how the PU joint works.\n");
printf ("A PU joint is a combination of a Universal joint and a Slider joint.\n");
printf ("It is a universal joint with a slider between the anchor point and \n");
printf ("body 1.\n\n");
printf ("The upper yellow body is fixed to the world\n");
printf ("The lower yellow body is attached to the upper body by a PU joint\n");
printf ("The green object is one aprt of the slider.\n");
printf ("The purple object is the second part of the slider.\n");
printf ("The red object represent the axis1 of the universal part. \n");
printf ("The blue object represent the axis2 of the universal part. \n");
printf ("The gray object represent the anchor2 of the PU joint. \n");
printKeyBoardShortCut();
}
// function to update camera position at each step.
void update()
{
// static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w");
// static int cnt = 0;
// char str[24];
// sprintf(str, "%06d",cnt++);
// dWorldExportDIF(world, file, str);
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd) {
case 'h' : case 'H' : case '?' :
printKeyBoardShortCut();
break;
// Force
case 'q' : case 'Q' :
dBodyAddForce(body[D],40,0,0);
break;
case 'w' : case 'W' :
dBodyAddForce(body[D],-40,0,0);
break;
case 'a' : case 'A' :
dBodyAddForce(body[D],0,40,0);
break;
case 's' : case 'S' :
dBodyAddForce(body[D],0,-40,0);
break;
case 'z' : case 'Z' :
dBodyAddForce(body[D],0,0,40);
break;
case 'x' : case 'X' :
dBodyAddForce(body[D],0,0,-40);
break;
// Torque
case 'e': case 'E':
dBodyAddTorque(body[D],0.1,0,0);
break;
case 'r': case 'R':
dBodyAddTorque(body[D],-0.1,0,0);
break;
case 'd': case 'D':
dBodyAddTorque(body[D],0, 0.1,0);
break;
case 'f': case 'F':
dBodyAddTorque(body[D],0,-0.1,0);
break;
case 'c': case 'C':
dBodyAddTorque(body[D],0,0,0.1);
break;
case 'v': case 'V':
dBodyAddTorque(body[D],0,0,0.1);
break;
// Velocity of joint
case ',': case '<' : {
dReal vel = joint->getParam (dParamVel3) - VEL_INC;
joint->setParam (dParamVel3, vel);
joint->setParam (dParamFMax3, 2);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case '.': case '>' : {
dReal vel = joint->getParam (dParamVel3) + VEL_INC;
joint->setParam (dParamVel3, vel);
joint->setParam (dParamFMax3, 2);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case 'l': case 'L' : {
dReal aLimit, lLimit, fmax;
if ( joint->getParam (dParamFMax) ) {
aLimit = dInfinity;
lLimit = dInfinity;
fmax = 0;
}
else {
aLimit = 0.25*PI;
lLimit = 0.5*axDim[LENGTH];
fmax = 0.02;
}
joint->setParam (dParamFMax1, fmax);
joint->setParam (dParamFMax2, fmax);
joint->setParam (dParamFMax3, fmax);
switch (joint->getType() ) {
case dJointTypePR : {
dPRJoint *pr = reinterpret_cast<dPRJoint *> (joint);
pr->setParam (dParamLoStop, -lLimit);
pr->setParam (dParamHiStop, -lLimit);
pr->setParam (dParamLoStop2, aLimit);
pr->setParam (dParamHiStop2, -aLimit);
}
break;
case dJointTypePU : {
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
pu->setParam (dParamLoStop1, -aLimit);
pu->setParam (dParamHiStop1, aLimit);
pu->setParam (dParamLoStop2, -aLimit);
pu->setParam (dParamHiStop2, aLimit);
pu->setParam (dParamLoStop3, -lLimit);
pu->setParam (dParamHiStop3, lLimit);
}
break;
default: {} // keep the compiler happy
}
}
break;
case 'g': case 'G' : {
dVector3 g;
dWorldGetGravity(world, g);
if ( g[2]< -0.1 )
dWorldSetGravity(world, 0, 0, 0);
else
dWorldSetGravity(world, 0, 0, -0.5);
}
break;
case 'p' :case 'P' : {
switch (joint->getType() ) {
case dJointTypeSlider : {
dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
std::cout<<"Position ="<<sj->getPosition() <<"\n";
}
break;
case dJointTypePU : {
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
std::cout<<"Position ="<<pu->getPosition() <<"\n";
std::cout<<"Position Rate="<<pu->getPositionRate() <<"\n";
std::cout<<"Angle1 ="<<pu->getAngle1() <<"\n";
std::cout<<"Angle1 Rate="<<pu->getAngle1Rate() <<"\n";
std::cout<<"Angle2 ="<<pu->getAngle2() <<"\n";
std::cout<<"Angle2 Rate="<<pu->getAngle2Rate() <<"\n";
}
break;
default: {} // keep the compiler happy
}
}
break;
}
}
static void drawBox (dGeomID id, int R, int G, int B)
{
if (!id)
return;
const dReal *pos = dGeomGetPosition (id);
const dReal *rot = dGeomGetRotation (id);
dsSetColor (R,G,B);
dVector3 l;
dGeomBoxGetLengths (id, l);
dsDrawBox (pos, rot, l);
}
// simulation loop
static void simLoop (int pause)
{
static bool todo = false;
if ( todo ) { // DEBUG
static int cnt = 0;
++cnt;
if (cnt == 5)
command ( 'q' );
if (cnt == 10)
dsStop();
}
if (!pause) {
double simstep = 0.01; // 10ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[W], 1,1,0);
drawBox (geom[EXT], 0,1,0);
dVector3 anchorPos;
dReal ang1 = 0;
dReal ang2 = 0;
dVector3 axisP, axisR1, axisR2;
if ( dJointTypePU == type ) {
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
ang1 = pu->getAngle1();
ang2 = pu->getAngle2();
pu->getAxis1 (axisR1);
pu->getAxis2 (axisR2);
pu->getAxisP (axisP);
dJointGetPUAnchor (pu->id(), anchorPos);
}
else if ( dJointTypePR == type ) {
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
pr->getAxis1 (axisP);
pr->getAxis2 (axisR1);
dJointGetPRAnchor (pr->id(), anchorPos);
}
// Draw the axisR
if ( geom[INT] ) {
dsSetColor (1,0,1);
dVector3 l;
dGeomBoxGetLengths (geom[INT], l);
const dReal *rotBox = dGeomGetRotation (geom[W]);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
dsDrawBox (pos, rotBox, l);
}
dsSetTexture (DS_CHECKERED);
if ( geom[AXIS1] ) {
dQuaternion q, qAng;
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dGeomGetQuaternion (geom[AXIS1], q);
dQuaternion qq;
dQMultiply1 (qq, qAng, q);
dMatrix3 R;
dQtoR (qq,R);
dGeomCylinderGetParams (geom[AXIS1], &radius, &length);
dsSetColor (1,0,0);
dsDrawCylinder (anchorPos, R, length, radius);
}
if ( dJointTypePU == type && geom[AXIS2] ) {
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
dQuaternion q, qAng, qq, qq1;
dGeomGetQuaternion (geom[AXIS2], q);
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
dQMultiply1 (qq, qAng, q);
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dQMultiply1 (qq1, qAng, qq);
dMatrix3 R;
dQtoR (qq1,R);
dGeomCylinderGetParams (geom[AXIS2], &radius, &length);
dsSetColor (0,0,1);
dsDrawCylinder (anchorPos, R, length, radius);
}
dsSetTexture (DS_WOOD);
// Draw the anchor
if ( geom[ANCHOR] ) {
dsSetColor (1,1,1);
dVector3 l;
dGeomBoxGetLengths (geom[ANCHOR], l);
const dReal *rotBox = dGeomGetRotation (geom[D]);
const dReal *posBox = dGeomGetPosition (geom[D]);
dVector3 e;
for (int i=0; i<3; ++i)
e[i] = posBox[i] - anchorPos[i];
dNormalize3 (e);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
dsDrawBox (pos, rotBox, l);
}
drawBox (geom[D], 1,1,0);
}
}
void Help (char **argv)
{
printf ("%s ", argv[0]);
printf (" -h | --help : print this help\n");
printf (" -p | --PRJoint : Use a PR joint instead of PU joint\n");
printf (" -t | --texture-path path : Path to the texture.\n");
printf (" Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
printf ("--------------------------------------------------\n");
printf ("Hit any key to continue:");
getchar();
exit (0);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
if (argc >= 2 ) {
for (int i=1; i < argc; ++i) {
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
type = dJointTypePR;
if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
int j = i+1;
if ( j >= argc || // Check if we have enough arguments
argv[j][0] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
}
dInitODE2(0);
world = dWorldCreate();
dWorldSetERP (world, 0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
// Create the body attached to the World
body[W] = dBodyCreate (world);
// Main axis of cylinder is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[W], body[W]);
dGeomSetCategoryBits (geom[W], catBits[W]);
dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
dBodySetMass (body[W], &m);
// Create the dandling body
body[D] = dBodyCreate (world);
// Main axis of capsule is along X=1
m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
m.adjust (Mass1);
geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
dGeomSetBody (geom[D], body[D]);
dGeomSetCategoryBits (geom[D], catBits[D]);
dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
dBodySetMass (body[D], &m);
// Create the external part of the slider joint
geom[EXT] = dCreateBox (0, extDim[X], extDim[Y], extDim[Z]);
dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
dGeomSetCollideBits (geom[EXT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
// Create the internal part of the slider joint
geom[INT] = dCreateBox (0, INT_EXT_RATIO*extDim[X],
INT_EXT_RATIO*extDim[Y],
INT_EXT_RATIO*extDim[Z]);
dGeomSetCategoryBits (geom[INT], catBits[INT]);
dGeomSetCollideBits (geom[INT],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
dMatrix3 R;
// Create the first axis of the universal joi9nt
//Rotation of 90deg around y
geom[AXIS1] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]);
dRFromAxisAndAngle(R, 0,1,0, 0.5*PI);
dGeomSetRotation(geom[AXIS1], R);
dGeomSetCategoryBits(geom[AXIS1], catBits[AXIS1]);
dGeomSetCollideBits(geom[AXIS1],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
// Create the second axis of the universal joint
geom[AXIS2] = dCreateCylinder(0, axDim[RADIUS], axDim[LENGTH]);
//Rotation of 90deg around y
dRFromAxisAndAngle(R, 1,0,0, 0.5*PI);
dGeomSetRotation(geom[AXIS2], R);
dGeomSetCategoryBits(geom[AXIS2], catBits[AXIS2]);
dGeomSetCollideBits(geom[AXIS2],
catBits[ALL] & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
// Create the anchor
geom[ANCHOR] = dCreateBox (0, ancDim[X], ancDim[Y], ancDim[Z]);
dGeomSetCategoryBits(geom[ANCHOR], catBits[ANCHOR]);
dGeomSetCollideBits(geom[ANCHOR],
catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );
if (body[W]) {
dBodySetPosition(body[W], 0, 0, 5);
}
if (geom[EXT]) {
dGeomSetPosition(geom[EXT], 0,0,3.8);
}
if (geom[INT]) {
dGeomSetPosition(geom[INT], 0,0,2.6);
}
if (geom[AXIS1]) {
dGeomSetPosition(geom[AXIS1], 0,0,2.5);
}
if (geom[AXIS2]) {
dGeomSetPosition(geom[AXIS2], 0,0,2.5);
}
if (geom[ANCHOR]) {
dGeomSetPosition(geom[ANCHOR], 0,0,2.25);
}
if (body[D]) {
dBodySetPosition(body[D], 0,0,1.5);
}
// Attache the upper box to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[W]);
dJointSetFixed (fixed );
if (type == dJointTypePR) {
dPRJoint *pr = new dPRJoint (world, 0);
pr->attach (body[W], body[D]);
pr->setAxis1 (0, 0, -1);
pr->setAxis2 (1, 0, 0);
joint = pr;
dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
}
else {
dPUJoint *pu = new dPUJoint (world, 0);
pu->attach (body[W], body[D]);
pu->setAxis1 (1, 0, 0);
pu->setAxis2 (0, 1, 0);
pu->setAxisP (0, 0, -1);
joint = pu;
dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
}
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

1090
thirdparty/ode-0.16.5/ode/demo/demo_joints.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

239
thirdparty/ode-0.16.5/ode/demo/demo_kinematic.cpp vendored Normal file
View File

@@ -0,0 +1,239 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <iostream>
#include <set>
#include <algorithm>
#include <functional>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawCylinder dsDrawCylinderD
#endif
using namespace std;
dWorld *world;
dSpace *space;
dPlane *ground;
dBody *kbody;
dBox *kbox;
dJointGroup joints;
dCylinder *kpole;
dBody *matraca;
dBox *matraca_geom;
dHingeJoint *hinge;
struct Box {
dBody body;
dBox geom;
Box() :
body(*world),
geom(*space, 0.2, 0.2, 0.2)
{
dMass mass;
mass.setBox(10, 0.2, 0.2, 0.2);
body.setMass(mass);
geom.setData(this);
geom.setBody(body);
}
void draw() const
{
dVector3 lengths;
geom.getLengths(lengths);
dsSetTexture(DS_WOOD);
dsSetColor(0,1,0);
dsDrawBox(geom.getPosition(), geom.getRotation(), lengths);
}
};
set<Box*> boxes;
set<Box*> to_remove;
void dropBox()
{
Box *box = new Box();
dReal px = (rand() / float(RAND_MAX)) * 2 - 1;
dReal py = (rand() / float(RAND_MAX)) * 2 - 1;
dReal pz = 2.5;
box->body.setPosition(px, py, pz);
boxes.insert(box);
}
void queueRemoval(dGeomID g)
{
Box *b = (Box*)dGeomGetData(g);
to_remove.insert(b);
}
void removeQueued()
{
while (!to_remove.empty()) {
Box *b = *to_remove.begin();
to_remove.erase(b);
boxes.erase(b);
delete b;
}
}
void nearCallback(void *, dGeomID g1, dGeomID g2)
{
if (g1 == ground->id()) {
queueRemoval(g2);
return;
}
if (g2 == ground->id()) {
queueRemoval(g1);
return;
}
dBodyID b1 = dGeomGetBody(g1);
dBodyID b2 = dGeomGetBody(g2);
if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
return;
const int MAX_CONTACTS = 10;
dContact contact[MAX_CONTACTS];
int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
for (int i=0; i<n; ++i) {
contact[i].surface.mode = 0;
contact[i].surface.mu = 1;
dJointID j = dJointCreateContact (*world, joints.id(), contact+i);
dJointAttach(j, b1, b2);
}
}
void
simLoop(int pause)
{
if (!pause) {
const dReal timestep = 0.04;
// this does a hard-coded circular motion animation
static float t=0;
t += timestep/4;
if (t > 2*M_PI)
t = 0;
dVector3 next_pos = { dCos(t), dSin(t), REAL(0.5)};
dVector3 vel;
// vel = (next_pos - cur_pos) / timestep
dSubtractVectors3(vel, next_pos, kbody->getPosition());
dScaleVector3(vel, 1/timestep);
kbody->setLinearVel(vel);
// end of hard-coded animation
space->collide(0, nearCallback);
removeQueued();
world->quickStep(timestep);
joints.clear();
}
dVector3 lengths;
// the moving platform
kbox->getLengths(lengths);
dsSetTexture(DS_WOOD);
dsSetColor(.3, .3, 1);
dsDrawBox(kbox->getPosition(), kbox->getRotation(), lengths);
dReal length, radius;
kpole->getParams(&radius, &length);
dsSetTexture(DS_CHECKERED);
dsSetColor(1, 1, 0);
dsDrawCylinder(kpole->getPosition(), kpole->getRotation(), length, radius);
// the matraca
matraca_geom->getLengths(lengths);
dsSetColor(1,0,0);
dsSetTexture(DS_WOOD);
dsDrawBox(matraca_geom->getPosition(), matraca_geom->getRotation(), lengths);
// and the boxes
for_each(boxes.begin(), boxes.end(), mem_fun(&Box::draw));
}
void command(int c)
{
switch (c) {
case ' ':
dropBox();
break;
}
}
int main(int argc, char **argv)
{
dInitODE();
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = 0;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
cout << endl << "*** Press SPACE to drop boxes **" << endl;
space = new dSimpleSpace();
ground = new dPlane(*space, 0, 0, 1, 0);
world = new dWorld;
world->setGravity(0, 0, -.5);
kbody = new dBody(*world);
kbody->setKinematic();
const dReal kx = 1, ky = 0, kz = .5;
kbody->setPosition(kx, ky, kz);
kbox = new dBox(*space, 3, 3, .5);
kbox->setBody(*kbody);
kpole = new dCylinder(*space, .125, 1.5);
kpole->setBody(*kbody);
dGeomSetOffsetPosition(kpole->id(), 0, 0, 0.8);
matraca = new dBody(*world);
matraca->setPosition(kx+0, ky+1, kz+1);
matraca_geom = new dBox(*space, 0.5, 2, 0.75);
matraca_geom->setBody(*matraca);
dMass mass;
mass.setBox(1, 0.5, 2, 0.75);
matraca->setMass(mass);
hinge = new dHingeJoint(*world);
hinge->attach(*kbody, *matraca);
hinge->setAnchor(kx, ky, kz+1);
hinge->setAxis(0, 0, 1);
dsSimulationLoop (argc, argv, 640, 480, &fn);
dCloseODE();
}

527
thirdparty/ode-0.16.5/ode/demo/demo_motion.cpp vendored Normal file
View File

@@ -0,0 +1,527 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
This demo shows how to use dContactMotionN in a lifting platform.
*/
//#include <unistd.h> // for usleep()
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawConvex dsDrawConvexD
#endif
// some constants
#define NUM 100 // max number of objects
#define DENSITY (5.0) // density of all objects
#define GPB 3 // maximum number of geometries per body
#define MAX_CONTACTS 8 // maximum number of contact points per body
#define USE_GEOM_OFFSET 1
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom[GPB]; // geometries representing this body
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int show_aabb = 0; // show geom AABBs?
static int show_contacts = 0; // show contact points?
static int random_pos = 1; // drop objects from random position?
static int write_world = 0;
static int show_body = 0;
static dGeomID platform, ground;
dVector3 platpos = {0, 0, 0};
int mov_type = 2;
dReal mov_time = 0;
const dReal mov1_speed = 0.2;
dVector3 mov2_vel = { 0.2, 0.1, 0.25};
/****************************************************************
* Movement 1: move platform up, reset every 80 units of time. *
* This is the simplest case *
****************************************************************/
static void moveplat_1(dReal stepsize)
{
mov_time += stepsize;
if (mov_time > 80)
mov_time = 0;
platpos[0] = platpos[1] = 0;
// the platform moves up (Z) at constant speed: mov1_speed
platpos[2] = mov1_speed * mov_time;
}
// Generate contact info for movement 1
static void contactplat_1(dContact &contact)
{
contact.surface.mode |= dContactMotionN;
contact.surface.motionN = mov1_speed;
}
/****************************************************************
* Movement 2: move platform along direction mov2_vel, reset *
* every 80 units of time. *
* This is the most general case: the geom moves along *
* an arbitrary direction. *
****************************************************************/
static void moveplat_2(dReal stepsize)
{
mov_time += stepsize;
if (mov_time > 80)
mov_time = 0;
// the platform moves at constant speed: mov2_speed
platpos[0] = mov2_vel[0] * mov_time;
platpos[1] = mov2_vel[1] * mov_time;
platpos[2] = mov2_vel[2] * mov_time;
}
// Generate contact info for movement 1
static void contactplat_2(dContact &contact)
{
/*
For arbitrary contact directions we need to project the moving
geom's velocity against the contact normal and fdir1, fdir2
(obtained with dPlaneSpace()). Assuming moving geom=g2
(so the contact joint is in the moving geom's reference frame):
motion1 = dCalcVectorDot3(fdir1, vel);
motion2 = dCalcVectorDot3(fdir2, vel);
motionN = dCalcVectorDot3(normal, vel);
For geom=g1 just negate motionN and motion2. fdir1 is an arbitrary
vector, so there's no need to negate motion1.
*/
contact.surface.mode |=
dContactMotionN | // velocity along normal
dContactMotion1 | dContactMotion2 | // and along the contact plane
dContactFDir1; // don't forget to set the direction 1
// This is a convenience function: given a vector, it finds other 2 perpendicular vectors
dVector3 motiondir1, motiondir2;
dPlaneSpace(contact.geom.normal, motiondir1, motiondir2);
for (int i=0; i<3; ++i)
contact.fdir1[i] = motiondir1[i];
dReal inv = 1;
if (contact.geom.g1 == platform)
inv = -1;
contact.surface.motion1 = dCalcVectorDot3(mov2_vel, motiondir1);
contact.surface.motion2 = inv * dCalcVectorDot3(mov2_vel, motiondir2);
contact.surface.motionN = inv * dCalcVectorDot3(mov2_vel, contact.geom.normal);
}
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
dMatrix3 RI;
static const dReal ss[3] = {0.02,0.02,0.02};
dContact contact[MAX_CONTACTS];
int numc = dCollide (o1, o2, MAX_CONTACTS,
&contact[0].geom, sizeof(dContact));
if (numc)
dRSetIdentity(RI);
bool isplatform = (o1 == platform) || (o2 == platform);
for (int i=0; i< numc; i++) {
contact[i].surface.mode = dContactBounce;
contact[i].surface.mu = 1;
contact[i].surface.bounce = 0.25;
contact[i].surface.bounce_vel = 0.01;
if (isplatform) {
switch (mov_type) {
case 1:
contactplat_1(contact[i]);
break;
case 2:
contactplat_2(contact[i]);
break;
}
}
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c, dGeomGetBody(o1), dGeomGetBody(o2));
if (show_contacts)
dsDrawBox (contact[i].geom.pos, RI, ss);
}
}
// start simulation - set viewpoint
static float xyz[3] = {2.1106f,-1.3007,2.f};
static float hpr[3] = {150.f,-13.5000f,0.0000f};
static void start()
{
//dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
printf ("To drop another object, press:\n");
printf (" b for box.\n");
printf (" s for sphere.\n");
printf (" c for capsule.\n");
printf (" y for cylinder.\n");
printf ("Press m to change the movement type\n");
printf ("Press space to reset the platform\n");
printf ("To toggle showing the geom AABBs, press a.\n");
printf ("To toggle showing the contact points, press t.\n");
printf ("To toggle dropping from random position/orientation, press r.\n");
printf ("To save the current state to 'state.dif', press 1.\n");
}
char locase (char c)
{
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void command (int cmd)
{
dsizeint i;
int k;
dReal sides[3];
dMass m;
int setBody;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'y')
{
setBody = 0;
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
if (obj[i].body) {
dBodyDestroy (obj[i].body);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) {
dGeomDestroy (obj[i].geom[k]);
}
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos)
{
dBodySetPosition (obj[i].body,
dRandReal()*2-1 + platpos[0],
dRandReal()*2-1 + platpos[1],
dRandReal()+2 + platpos[2]);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else
{
dBodySetPosition (obj[i].body,
platpos[0],
platpos[1],
platpos[2]+2);
dRSetIdentity (R);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
else if (cmd == 'y') {
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
if (!setBody)
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) {
dGeomSetBody (obj[i].geom[k],obj[i].body);
}
}
dBodySetMass (obj[i].body,&m);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
else if (cmd == ' ') {
mov_time = 0;
}
else if (cmd == 'm') {
mov_type = mov_type==1 ? 2 : 1;
mov_time = 0;
}
}
// draw a geom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
if (show_body) {
dBodyID body = dGeomGetBody(g);
if (body) {
const dReal *bodypos = dBodyGetPosition (body);
const dReal *bodyr = dBodyGetRotation (body);
dReal bodySides[3] = { 0.1, 0.1, 0.1 };
dsSetColorAlpha(0,1,0,1);
dsDrawBox(bodypos,bodyr,bodySides);
}
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
// simulation loop
static void updatecam()
{
xyz[0] = platpos[0] + 3.3;
xyz[1] = platpos[1] - 1.8;
xyz[2] = platpos[2] + 2;
dsSetViewpoint (xyz, hpr);
}
static void simLoop (int pause)
{
const dReal stepsize = 0.02;
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) {
if (mov_type == 1)
moveplat_1(stepsize);
else
moveplat_2(stepsize);
dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]);
updatecam();
dWorldQuickStep (world,stepsize);
//dWorldStep (world,stepsize);
}
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0.8,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
dsSetColor (1,0,0);
drawGeom (platform,0,0,show_aabb);
//usleep(5000);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE();
world = dWorldCreate();
#if 1
space = dHashSpaceCreate (0);
#elif 0
dVector3 center = {0,0,0}, extents = { 100, 100, 100};
space = dQuadTreeSpaceCreate(0, center, extents, 5);
#elif 0
space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
#else
space = dSimpleSpaceCreate(0);
#endif
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dWorldSetLinearDamping(world, 0.00001);
dWorldSetAngularDamping(world, 0.005);
dWorldSetMaxAngularSpeed(world, 200);
dWorldSetContactSurfaceLayer (world,0.001);
ground = dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// create lift platform
platform = dCreateBox(space, 4, 4, 1);
dGeomSetCategoryBits(ground, 1ul);
dGeomSetCategoryBits(platform, 2ul);
dGeomSetCollideBits(ground, ~2ul);
dGeomSetCollideBits(platform, ~1ul);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
// Local Variables:
// c-basic-offset:4
// End:

209
thirdparty/ode-0.16.5/ode/demo/demo_motor.cpp vendored Normal file
View File

@@ -0,0 +1,209 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#endif
// some constants
#define SIDE (0.5f) // side length of a box
#define MASS (1.0) // mass of a box
// dynamics and collision objects
static dWorldID world;
static dBodyID body[2];
static dGeomID geom[2];
static dJointID lmotor[2];
static dJointID amotor[2];
static dSpaceID space;
static dJointGroupID contactgroup;
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press 'q,a,z' to control one axis of lmotor connectiong two bodies. (q is +,a is 0, z is -)\n");
printf ("Press 'w,e,r' to control one axis of lmotor connectiong first body with world. (w is +,e is 0, r is -)\n");
}
// called when a key pressed
static void command (int cmd)
{
if (cmd == 'q' || cmd == 'Q') {
dJointSetLMotorParam(lmotor[0],dParamVel,0);
dJointSetLMotorParam(lmotor[0],dParamVel2,0);
dJointSetLMotorParam(lmotor[0],dParamVel3,0.1);
} else if (cmd == 'a' || cmd == 'A') {
dJointSetLMotorParam(lmotor[0],dParamVel,0);
dJointSetLMotorParam(lmotor[0],dParamVel2,0);
dJointSetLMotorParam(lmotor[0],dParamVel3,0);
} else if (cmd == 'z' || cmd == 'Z') {
dJointSetLMotorParam(lmotor[0],dParamVel,0);
dJointSetLMotorParam(lmotor[0],dParamVel2,0);
dJointSetLMotorParam(lmotor[0],dParamVel3,-0.1);
} else if (cmd == 'w' || cmd == 'W') {
dJointSetLMotorParam(lmotor[1],dParamVel,0.1);
dJointSetLMotorParam(lmotor[1],dParamVel2,0);
dJointSetLMotorParam(lmotor[1],dParamVel3,0);
} else if (cmd == 'e' || cmd == 'E') {
dJointSetLMotorParam(lmotor[1],dParamVel,0);
dJointSetLMotorParam(lmotor[1],dParamVel2,0);
dJointSetLMotorParam(lmotor[1],dParamVel3,0);
} else if (cmd == 'r' || cmd == 'R') {
dJointSetLMotorParam(lmotor[1],dParamVel,-0.1);
dJointSetLMotorParam(lmotor[1],dParamVel2,0);
dJointSetLMotorParam(lmotor[1],dParamVel3,0);
}
}
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
dContact contact;
contact.surface.mode = 0;
contact.surface.mu = dInfinity;
if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
dJointID c = dJointCreateContact (world,contactgroup,&contact);
dJointAttach (c,b1,b2);
}
}
// simulation loop
static void simLoop (int pause)
{
if (!pause) {
dSpaceCollide(space,0,&nearCallback);
dWorldQuickStep (world,0.05);
dJointGroupEmpty(contactgroup);
}
dReal sides1[3];
dGeomBoxGetLengths(geom[0], sides1);
dReal sides2[3];
dGeomBoxGetLengths(geom[1], sides2);
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
contactgroup = dJointGroupCreate(0);
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0,0,1);
geom[0] = dCreateBox(space,SIDE,SIDE,SIDE);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1],0,0,2);
geom[1] = dCreateBox(space,SIDE,SIDE,SIDE);
dGeomSetBody(geom[0],body[0]);
dGeomSetBody(geom[1],body[1]);
lmotor[0] = dJointCreateLMotor (world,0);
dJointAttach (lmotor[0],body[0],body[1]);
lmotor[1] = dJointCreateLMotor (world,0);
dJointAttach (lmotor[1],body[0],0);
amotor[0] = dJointCreateAMotor(world,0);
dJointAttach(amotor[0], body[0],body[1]);
amotor[1] = dJointCreateAMotor(world,0);
dJointAttach(amotor[1], body[0], 0);
for (int i=0; i<2; i++) {
dJointSetAMotorNumAxes(amotor[i], 3);
dJointSetAMotorAxis(amotor[i],0,1,1,0,0);
dJointSetAMotorAxis(amotor[i],1,1,0,1,0);
dJointSetAMotorAxis(amotor[i],2,1,0,0,1);
dJointSetAMotorParam(amotor[i],dParamFMax,0.00001);
dJointSetAMotorParam(amotor[i],dParamFMax2,0.00001);
dJointSetAMotorParam(amotor[i],dParamFMax3,0.00001);
dJointSetAMotorParam(amotor[i],dParamVel,0);
dJointSetAMotorParam(amotor[i],dParamVel2,0);
dJointSetAMotorParam(amotor[i],dParamVel3,0);
dJointSetLMotorNumAxes(lmotor[i],3);
dJointSetLMotorAxis(lmotor[i],0,1,1,0,0);
dJointSetLMotorAxis(lmotor[i],1,1,0,1,0);
dJointSetLMotorAxis(lmotor[i],2,1,0,0,1);
dJointSetLMotorParam(lmotor[i],dParamFMax,0.0001);
dJointSetLMotorParam(lmotor[i],dParamFMax2,0.0001);
dJointSetLMotorParam(lmotor[i],dParamFMax3,0.0001);
}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy(contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

415
thirdparty/ode-0.16.5/ode/demo/demo_moving_convex.cpp vendored Normal file
View File

@@ -0,0 +1,415 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#include "bunny_geom.h"
#include "convex_bunny_geom.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#define dsDrawTriangle dsDrawTriangleD
#define dsDrawConvex dsDrawConvexD
#endif
// some constants
#define NUM 200 // max number of objects
#define DENSITY (5.0) // density of all objects
#define GPB 3 // maximum number of geometries per body
#define MAX_CONTACTS 64 // maximum number of contact points per body
// dynamics and collision objects
struct MyObject
{
dBodyID body; // the body
dGeomID geom[GPB]; // geometries representing this body
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1; // selected object
static int show_aabb = 0; // show geom AABBs?
static int show_contacts = 0; // show contact points?
static int random_pos = 1; // drop objects from random position?
typedef dReal dVector3R[3];
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback( void *, dGeomID o1, dGeomID o2 )
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody( o1 );
dBodyID b2 = dGeomGetBody( o2 );
if ( b1 && b2 && dAreConnectedExcluding( b1,b2,dJointTypeContact ) ) return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for ( i=0; i<MAX_CONTACTS; i++ )
{
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
if ( int numc = dCollide( o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof( dContact ) ) )
{
dMatrix3 RI;
dRSetIdentity( RI );
const dReal ss[3] = {0.02,0.02,0.02};
for ( i=0; i<numc; i++ )
{
dJointID c = dJointCreateContact( world,contactgroup,contact+i );
dJointAttach( c,b1,b2 );
if ( show_contacts ) dsDrawBox( contact[i].geom.pos,RI,ss );
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread( dAllocateMaskAll );
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint( xyz,hpr );
printf( "To drop another object, press:\n" );
printf( " b for box.\n" );
printf( " s for sphere.\n" );
printf( " c for capsule.\n" );
printf( " v for a convex.\n" );
printf( "To select an object, press space.\n" );
printf( "To disable the selected object, press d.\n" );
printf( "To enable the selected object, press e.\n" );
printf( "To toggle showing the geom AABBs, press a.\n" );
printf( "To toggle showing the contact points, press t.\n" );
printf( "To toggle dropping from random position/orientation, press r.\n" );
}
char locase( char c )
{
if ( c >= 'A' && c <= 'Z' ) return c - ( 'a'-'A' );
else return c;
}
// called when a key pressed
static void command( int cmd )
{
int i,k;
dReal sides[3];
dMass m;
cmd = locase( cmd );
if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' || cmd == 'y')
{
if ( num < NUM )
{
i = num;
num++;
}
else
{
i = nextobj;
nextobj++;
if ( nextobj >= num ) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy( obj[i].body );
for ( k=0; k < GPB; k++ )
{
if ( obj[i].geom[k] ) dGeomDestroy( obj[i].geom[k] );
}
memset( &obj[i],0,sizeof( obj[i] ) );
}
obj[i].body = dBodyCreate( world );
for ( k=0; k<3; k++ ) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if ( random_pos )
{
dBodySetPosition( obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3 );
dRFromAxisAndAngle( R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0 );
}
else
{
dReal maxheight = 0;
for ( k=0; k<num; k++ )
{
const dReal *pos = dBodyGetPosition( obj[k].body );
if ( pos[2] > maxheight ) maxheight = pos[2];
}
dBodySetPosition( obj[i].body, 0,0,maxheight+1 );
dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 );
}
dBodySetRotation( obj[i].body,R );
dBodySetData( obj[i].body,( void* )( dsizeint )i );
if ( cmd == 'b' )
{
dMassSetBox( &m,DENSITY,sides[0],sides[1],sides[2] );
obj[i].geom[0] = dCreateBox( space,sides[0],sides[1],sides[2] );
}
else if ( cmd == 'c' )
{
sides[0] *= 0.5;
dMassSetCapsule( &m,DENSITY,3,sides[0],sides[1] );
obj[i].geom[0] = dCreateCapsule( space,sides[0],sides[1] );
}
else if (cmd == 'y') {
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if ( cmd == 's' )
{
sides[0] *= 0.5;
dMassSetSphere( &m,DENSITY,sides[0] );
obj[i].geom[0] = dCreateSphere( space,sides[0] );
}
else if ( cmd == 'v' )
{
obj[i].geom[0] = dCreateConvex( space,
convexBunnyPlanes,
convexBunnyPlaneCount,
convexBunnyPoints,
convexBunnyPointCount,
convexBunnyPolygons );
/// Use equivalent TriMesh to set mass
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount,
( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) );
dGeomTriMeshDataPreprocess2( new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL );
dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 );
dMassSetTrimesh( &m, DENSITY, triMesh );
dGeomDestroy( triMesh );
dGeomTriMeshDataDestroy( new_tmdata );
printf( "mass at %f %f %f\n", m.c[0], m.c[1], m.c[2] );
dGeomSetPosition( obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2] );
dMassTranslate( &m, -m.c[0], -m.c[1], -m.c[2] );
}
for ( k=0; k < GPB; k++ )
{
if ( obj[i].geom[k] ) dGeomSetBody( obj[i].geom[k],obj[i].body );
}
dBodySetMass( obj[i].body,&m );
}
if ( cmd == ' ' )
{
selected++;
if ( selected >= num ) selected = 0;
if ( selected < 0 ) selected = 0;
}
else if ( cmd == 'd' && selected >= 0 && selected < num )
{
dBodyDisable( obj[selected].body );
}
else if ( cmd == 'e' && selected >= 0 && selected < num )
{
dBodyEnable( obj[selected].body );
}
else if ( cmd == 'a' )
{
show_aabb ^= 1;
}
else if ( cmd == 't' )
{
show_contacts ^= 1;
}
else if ( cmd == 'r' )
{
random_pos ^= 1;
}
}
// draw a geom
void drawGeom( dGeomID g, const dReal *pos, const dReal *R, int show_aabb )
{
if ( !g ) return;
if ( !pos ) pos = dGeomGetPosition( g );
if ( !R ) R = dGeomGetRotation( g );
int type = dGeomGetClass( g );
if ( type == dBoxClass )
{
dVector3 sides;
dGeomBoxGetLengths( g,sides );
dsDrawBox( pos,R,sides );
}
else if ( type == dSphereClass )
{
dsDrawSphere( pos,R,dGeomSphereGetRadius( g ) );
}
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if ( type == dCapsuleClass )
{
dReal radius,length;
dGeomCapsuleGetParams( g,&radius,&length );
dsDrawCapsule( pos,R,length,radius );
}
else if ( type == dConvexClass )
{
dsDrawConvex( pos,R,
convexBunnyPlanes,
convexBunnyPlaneCount,
convexBunnyPoints,
convexBunnyPointCount,
convexBunnyPolygons );
}
if ( show_aabb )
{
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB( g,aabb );
dVector3 bbpos;
for ( int i=0; i<3; i++ ) bbpos[i] = 0.5*( aabb[i*2] + aabb[i*2+1] );
dVector3 bbsides;
for ( int j=0; j<3; j++ ) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity( RI );
dsSetColorAlpha( 1,0,0,0.5 );
dsDrawBox( bbpos,RI,bbsides );
}
}
// simulation loop
static void simLoop( int pause )
{
dsSetColor( 0,0,2 );
dSpaceCollide( space,0,&nearCallback );
if ( !pause ) dWorldQuickStep( world,0.05 );
for ( int j = 0; j < dSpaceGetNumGeoms( space ); j++ )
{
dSpaceGetGeom( space, j );
}
// remove all contact joints
dJointGroupEmpty( contactgroup );
dsSetColor( 1,1,0 );
dsSetTexture( DS_WOOD );
for ( int i=0; i<num; i++ )
{
for ( int j=0; j < GPB; j++ )
{
if ( obj[i].geom[j] )
{
if ( i==selected )
{
dsSetColor( 0,0.7,1 );
}
else if ( ! dBodyIsEnabled( obj[i].body ) )
{
dsSetColor( 1,0,0 );
}
else
{
dsSetColor( 1,1,0 );
}
drawGeom( obj[i].geom[j],0,0,show_aabb );
}
}
}
}
int main( int argc, char **argv )
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2( 0 );
world = dWorldCreate();
space = dSimpleSpaceCreate( 0 );
contactgroup = dJointGroupCreate( 0 );
dWorldSetGravity( world,0,0,-0.5 );
dWorldSetCFM( world,1e-5 );
dCreatePlane( space,0,0,1,0 );
memset( obj,0,sizeof( obj ) );
// run simulation
dsSimulationLoop( argc,argv,352,288,&fn );
dJointGroupDestroy( contactgroup );
dSpaceDestroy( space );
dWorldDestroy( world );
dCloseODE();
return 0;
}

677
thirdparty/ode-0.16.5/ode/demo/demo_moving_trimesh.cpp vendored Normal file
View File

@@ -0,0 +1,677 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#include "bunny_geom.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
//<---- Convex Object
static const dReal planes[] = // planes for a cube
{
1.0f ,0.0f ,0.0f ,0.25f,
0.0f ,1.0f ,0.0f ,0.25f,
0.0f ,0.0f ,1.0f ,0.25f,
0.0f ,0.0f ,-1.0f,0.25f,
0.0f ,-1.0f,0.0f ,0.25f,
-1.0f,0.0f ,0.0f ,0.25f
/*
1.0f ,0.0f ,0.0f ,2.0f,
0.0f ,1.0f ,0.0f ,1.0f,
0.0f ,0.0f ,1.0f ,1.0f,
0.0f ,0.0f ,-1.0f,1.0f,
0.0f ,-1.0f,0.0f ,1.0f,
-1.0f,0.0f ,0.0f ,0.0f
*/
};
static const unsigned int planecount=6;
static const dReal points[] = // points for a cube
{
0.25f,0.25f,0.25f,
-0.25f,0.25f,0.25f,
0.25f,-0.25f,0.25f,
-0.25f,-0.25f,0.25f,
0.25f,0.25f,-0.25f,
-0.25f,0.25f,-0.25f,
0.25f,-0.25f,-0.25f,
-0.25f,-0.25f,-0.25f,
};
static const unsigned int pointcount=8;
static const unsigned int polygons[] = //Polygons for a cube (6 squares)
{
4,0,2,6,4, // positive X
4,1,0,4,5, // positive Y
4,0,1,3,2, // positive Z
4,3,1,5,7, // negative X
4,2,3,7,6, // negative Y
4,5,4,6,7, // negative Z
};
//----> Convex Object
int tmTriangles[] =
{
0,2,6,
0,6,4,
1,0,4,
1,4,5,
0,1,3,
0,3,2,
3,1,5,
3,5,7,
2,3,7,
2,7,6,
5,4,6,
5,6,7
};
float tmVertices[] =
{
0.25f,0.25f,0.25f, // point 0
-0.25f,0.25f,0.25f, // point 1
0.25f,-0.25f,0.25f, // point 2
-0.25f,-0.25f,0.25f,// point 3
0.25f,0.25f,-0.25f, // point 4
-0.25f,0.25f,-0.25f,// point 5
0.25f,-0.25f,-0.25f,// point 6
-0.25f,-0.25f,-0.25f,// point 7
};
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#define dsDrawTriangle dsDrawTriangleD
#define dsDrawConvex dsDrawConvexD
#endif
// some constants
#define NUM 200 // max number of objects
#define DENSITY (5.0) // density of all objects
#define GPB 3 // maximum number of geometries per body
#define MAX_CONTACTS 64 // maximum number of contact points per body
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom[GPB]; // geometries representing this body
// Trimesh only - double buffered matrices for 'last transform' setup
dReal matrix_dblbuff[ 16 * 2 ];
int last_matrix_index;
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1; // selected object
static int show_aabb = 0; // show geom AABBs?
static int show_contacts = 0; // show contact points?
static int random_pos = 1; // drop objects from random position?
typedef dReal dVector3R[3];
dGeomID TriMesh1;
dGeomID TriMesh2;
static dTriMeshDataID TriData1, TriData2; // reusable static trimesh data
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i<MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("To drop another object, press:\n");
printf (" b for box.\n");
printf (" s for sphere.\n");
printf (" y for cylinder.\n");
printf (" c for capsule.\n");
printf (" x for a composite object.\n");
printf (" v for a convex object.\n");
printf (" m for a trimesh.\n");
printf ("To select an object, press space.\n");
printf ("To disable the selected object, press d.\n");
printf ("To enable the selected object, press e.\n");
printf ("To toggle showing the geom AABBs, press a.\n");
printf ("To toggle showing the contact points, press t.\n");
printf ("To toggle dropping from random position/orientation, press r.\n");
}
char locase (char c)
{
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(dsizeint)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
else if (cmd == 'y') {
sides[1] *= 0.5;
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'm') {
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
(dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
dGeomSetData(obj[i].geom[0], new_tmdata);
dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
// draw a geom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
} else if (type == dConvexClass) {
//dVector3 sides={0.50,0.50,0.50};
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
// set previous transformation matrix for trimesh
void setCurrentTransform(dGeomID geom)
{
const dReal* Pos = dGeomGetPosition(geom);
const dReal* Rot = dGeomGetRotation(geom);
const dReal Transform[16] =
{
Rot[0], Rot[4], Rot[8], 0,
Rot[1], Rot[5], Rot[9], 0,
Rot[2], Rot[6], Rot[10], 0,
Pos[0], Pos[1], Pos[2], 1
};
dGeomTriMeshSetLastTransform( geom, *(dMatrix4*)(&Transform) );
}
// simulation loop
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
#if 1
// What is this for??? - Bram
if (!pause)
{
for (int i=0; i<num; i++)
for (int j=0; j < GPB; j++)
if (obj[i].geom[j])
if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass)
setCurrentTransform(obj[i].geom[j]);
setCurrentTransform(TriMesh1);
setCurrentTransform(TriMesh2);
}
#endif
//if (!pause) dWorldStep (world,0.05);
if (!pause) dWorldQuickStep (world,0.05);
for (int j = 0; j < dSpaceGetNumGeoms(space); j++){
dSpaceGetGeom(space, j);
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (obj[i].geom[j]) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) {
dTriIndex* Indices = (dTriIndex*)::Indices;
// assume all trimeshes are drawn as bunnies
const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
for (int ii = 0; ii < IndexCount / 3; ii++) {
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[ii * 3 + 0] * 3 + 0],
Vertices[Indices[ii * 3 + 0] * 3 + 1],
Vertices[Indices[ii * 3 + 0] * 3 + 2],
Vertices[Indices[ii * 3 + 1] * 3 + 0],
Vertices[Indices[ii * 3 + 1] * 3 + 1],
Vertices[Indices[ii * 3 + 1] * 3 + 2],
Vertices[Indices[ii * 3 + 2] * 3 + 0],
Vertices[Indices[ii * 3 + 2] * 3 + 1],
Vertices[Indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}
// tell the tri-tri collider the current transform of the trimesh --
// this is fairly important for good results.
// Fill in the (4x4) matrix.
dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );
p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0;
p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0;
p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0;
p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1;
// Flip to other matrix.
obj[i].last_matrix_index = !obj[i].last_matrix_index;
dGeomTriMeshSetLastTransform( obj[i].geom[j],
*(dMatrix4*)( obj[i].matrix_dblbuff + obj[i].last_matrix_index * 16 ) );
} else {
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
}
dTriIndex* Indices = (dTriIndex*)::Indices;
{const dReal* Pos = dGeomGetPosition(TriMesh1);
const dReal* Rot = dGeomGetRotation(TriMesh1);
{for (int i = 0; i < IndexCount / 3; i++){
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[i * 3 + 0] * 3 + 0],
Vertices[Indices[i * 3 + 0] * 3 + 1],
Vertices[Indices[i * 3 + 0] * 3 + 2],
Vertices[Indices[i * 3 + 1] * 3 + 0],
Vertices[Indices[i * 3 + 1] * 3 + 1],
Vertices[Indices[i * 3 + 1] * 3 + 2],
Vertices[Indices[i * 3 + 2] * 3 + 0],
Vertices[Indices[i * 3 + 2] * 3 + 1],
Vertices[Indices[i * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0);
}}}
{const dReal* Pos = dGeomGetPosition(TriMesh2);
const dReal* Rot = dGeomGetRotation(TriMesh2);
{for (int i = 0; i < IndexCount / 3; i++){
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[i * 3 + 0] * 3 + 0],
Vertices[Indices[i * 3 + 0] * 3 + 1],
Vertices[Indices[i * 3 + 0] * 3 + 2],
Vertices[Indices[i * 3 + 1] * 3 + 0],
Vertices[Indices[i * 3 + 1] * 3 + 1],
Vertices[Indices[i * 3 + 1] * 3 + 2],
Vertices[Indices[i * 3 + 2] * 3 + 0],
Vertices[Indices[i * 3 + 2] * 3 + 1],
Vertices[Indices[i * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}}}
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
// note: can't share tridata if intending to trimesh-trimesh collide
const unsigned preprocessFlags = (1U << dTRIDATAPREPROCESS_BUILD_CONCAVE_EDGES) | (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES);
TriData1 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(TriData1, preprocessFlags, NULL);
TriData2 = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(TriData2, preprocessFlags, NULL);
TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
dGeomSetData(TriMesh1, TriData1);
dGeomSetData(TriMesh2, TriData2);
{dGeomSetPosition(TriMesh1, 0, 0, 0.9);
dMatrix3 Rotation;
dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
dGeomSetRotation(TriMesh1, Rotation);}
{dGeomSetPosition(TriMesh2, 1, 0, 0.9);
dMatrix3 Rotation;
dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
dGeomSetRotation(TriMesh2, Rotation);}
dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
// dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dThreadingImplementationShutdownProcessing(threading);
dThreadingFreeThreadPool(pool);
dWorldSetStepThreadingImplementation(world, NULL, NULL);
dThreadingFreeImplementation(threading);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

1380
thirdparty/ode-0.16.5/ode/demo/demo_ode.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

813
thirdparty/ode-0.16.5/ode/demo/demo_piston.cpp vendored Normal file
View File

@@ -0,0 +1,813 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
* Created by: Remi Ricard *
* (remi.ricard@simlog.com or papaDoc@videotron.ca) *
* Creation date: 2007/05/04 *
*************************************************************************/
/*
This program demonstrates how the Piston joint works.
A Piston joint enables the sliding of a body with respect to another body
and the 2 bodies are free to rotate about the sliding axis.
- The yellow body is fixed to the world.
- The yellow body and the blue body are attached by a Piston joint with
the axis along the x direction.
- The purple object is a geometry obstacle.
- The red line is the representation of the prismatic axis
- The orange line is the representation of the rotoide axis
- The light blue ball is the anchor position
N.B. Many command options are available type -h to print them.
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <iostream>
#include <math.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawSphere dsDrawSphereD
#endif
const dReal VEL_INC = 0.01; // Velocity increment
// physics parameters
const dReal PI = 3.14159265358979323846264338327950288419716939937510;
const dReal BODY1_LENGTH = 1.5; // Size along the X axis
const dReal RADIUS = 0.2;
const dReal AXIS_RADIUS = 0.01;
#define X 0
#define Y 1
#define Z 2
enum INDEX
{
BODY1 = 0,
BODY2,
RECT,
BOX,
OBS,
GROUND,
NUM_PARTS,
ALL = NUM_PARTS
};
const int catBits[NUM_PARTS+1] =
{
0x0001, ///< Ext Cylinder category
0x0002, ///< Int Cylinder category
0x0004, ///< Int_Rect Cylinder category
0x0008, ///< Box category
0x0010, ///< Obstacle category
0x0020, ///< Ground category
~0L ///< All categories
};
#define Mass1 10
#define Mass2 8
//camera view
static float xyz[3] = {2.0f,-3.5f,2.0000f};
static float hpr[3] = {90.000f,-25.5000f,0.0000f};
//world,space,body & geom
static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;
static dBodyID body[NUM_PARTS];
static dGeomID geom[NUM_PARTS];
// Default Positions and anchor of the 2 bodies
dVector3 pos1;
dVector3 pos2;
dVector3 anchor;
static dJoint *joint;
const dReal BODY2_SIDES[3] = {0.4, 0.4, 0.4};
const dReal OBS_SIDES[3] = {1,1,1};
const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2};
int type = dJointTypePiston;
//#pragma message("tc to be changed to 0")
int tc = 0; // The test case choice;
//collision detection
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i,n;
dBodyID b1 = dGeomGetBody (o1);
dBodyID b2 = dGeomGetBody (o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact) ) return;
const int N = 10;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) );
if (n > 0)
{
for (i=0; i<n; i++)
{
contact[i].surface.mode = (dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM |
dContactApprox1);
contact[i].surface.mu = 0.1;
contact[i].surface.slip1 = 0.02;
contact[i].surface.slip2 = 0.02;
contact[i].surface.soft_erp = 0.1;
contact[i].surface.soft_cfm = 0.0001;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) );
}
}
}
static void printKeyBoardShortCut()
{
printf ("Press 'h' for this help.\n");
printf ("Press 'q' to add force on BLUE body along positive x direction.\n");
printf ("Press 'w' to add force on BLUE body along negative x direction.\n");
printf ("Press 'a' to add force on BLUE body along positive y direction.\n");
printf ("Press 's' to add force on BLUE body along negative y direction.\n");
printf ("Press 'z' to add force on BLUE body along positive z direction.\n");
printf ("Press 'x' to add force on BLUE body along negative z direction.\n");
printf ("Press 'e' to add torque on BLUE body around positive x direction \n");
printf ("Press 'r' to add torque on BLUE body around negative x direction \n");
printf ("Press 'd' to add torque on BLUE body around positive y direction \n");
printf ("Press 'f' to add torque on BLUE body around negative y direction \n");
printf ("Press 'c' to add torque on BLUE body around positive z direction \n");
printf ("Press 'v' to add torque on BLUE body around negative z direction \n");
printf ("Press 't' to add force on prismatic joint in the positive axis direction\n");
printf ("Press 'y' to add force on prismatic joint in the negative axis direction\n");
printf ("Press 'i' to add limits on the prismatic joint (0 to 0) \n");
printf ("Press 'o' to add limits on the rotoide joint (0 to 0)\n");
printf ("Press 'k' to add limits on the rotoide joint (-45 to 45deg) \n");
printf ("Press 'l' to remove limits on the rotoide joint \n");
printf ("Press '.' to increase joint velocity along the prismatic direction.\n");
printf ("Press ',' to decrease joint velocity along the prismatic direction.\n");
printf ("Press 'p' to print the Position of the joint.\n");
printf ("Press '+' Go to the next test case.\n");
printf ("Press '-' Go to the previous test case.\n");
printf ("Press '8' To remove one of the body. The blue body and the world will be\n");
printf (" attached to the joint (blue body at position 1)\n");
printf ("Press '9' To remove one of the body. The blue body and the world will be\n");
printf (" attached to the joint (body body at position 2)\n");
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint (xyz,hpr);
printf ("This program demonstrates how the Piston joint works.\n");
printf ("A Piston joint enables the sliding of a body with respect to another body\n");
printf ("and the 2 bodies are free to rotate about the sliding axis.\n\n");
printf ("The yellow body is fixed to the world\n");
printf ("The yellow body and the blue body are attached by a Piston joint with\n");
printf ("the axis along the x direction.\n");
printf ("The purple object is a geometry obstacle.\n");
printKeyBoardShortCut();
}
void setPositionBodies (int val)
{
const dVector3 POS1 = {0,0,1.5,0};
const dVector3 POS2 = {0,0,1.5,0};
const dVector3 ANCHOR = {0,0,1.5,0};
for (int i=0; i<3; ++i)
{
pos1[i] = POS1[i];
pos2[i] = POS2[i];
anchor[i] = ANCHOR[i];
}
if (body[BODY1])
{
dBodySetLinearVel (body[BODY1], 0,0,0);
dBodySetAngularVel (body[BODY1], 0,0,0);
}
if (body[BODY2])
{
dBodySetLinearVel (body[BODY2], 0,0,0);
dBodySetAngularVel (body[BODY2], 0,0,0);
}
switch (val)
{
case 3:
pos1[Z] += -0.5;
anchor[Z] -= 0.25;
break;
case 2:
pos1[Z] -= 0.5;
anchor[Z] -= 0.5;
break;
case 1:
pos1[Z] += -0.5;
break;
default: // This is also case 0
// Nothing to be done
break;
}
const dMatrix3 R =
{
1,0,0,0,
0,1,0,0,
0,0,1,0
};
if (body[BODY1])
{
dBodySetPosition (body[BODY1], pos1[X], pos1[Y], pos1[Z]);
dBodySetRotation (body[BODY1], R);
}
if (body[BODY2])
{
dBodySetPosition (body[BODY2], pos2[X], pos2[Y], pos2[Z]);
dBodySetRotation (body[BODY2], R);
}
if (joint)
{
joint->attach (body[BODY1], body[BODY2]);
if (joint->getType() == dJointTypePiston)
dJointSetPistonAnchor(joint->id(), anchor[X], anchor[Y], anchor[Z]);
}
}
// function to update camera position at each step.
void update()
{
// static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w");
// static int cnt = 0;
// char str[24];
// sprintf(str, "%06d",cnt++);
// dWorldExportDIF(world, file, str);
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd)
{
case 'h' :
case 'H' :
case '?' :
printKeyBoardShortCut();
break;
// Force
case 'q' :
case 'Q' :
dBodyAddForce (body[BODY1],4,0,0);
break;
case 'w' :
case 'W' :
dBodyAddForce (body[BODY1],-4,0,0);
break;
case 'a' :
case 'A' :
dBodyAddForce (body[BODY1],0,40,0);
break;
case 's' :
case 'S' :
dBodyAddForce (body[BODY1],0,-40,0);
break;
case 'z' :
case 'Z' :
dBodyAddForce (body[BODY1],0,0,4);
break;
case 'x' :
case 'X' :
dBodyAddForce (body[BODY1],0,0,-4);
break;
// Torque
case 'e':
case 'E':
dBodyAddTorque (body[BODY1],0.1,0,0);
break;
case 'r':
case 'R':
dBodyAddTorque (body[BODY1],-0.1,0,0);
break;
case 'd':
case 'D':
dBodyAddTorque (body[BODY1],0, 0.1,0);
break;
case 'f':
case 'F':
dBodyAddTorque (body[BODY1],0,-0.1,0);
break;
case 'c':
case 'C':
dBodyAddTorque (body[BODY1],0.1,0,0);
break;
case 'v':
case 'V':
dBodyAddTorque (body[BODY1],-0.1,0,0);
break;
case 't':
case 'T':
if (joint->getType() == dJointTypePiston)
dJointAddPistonForce (joint->id(),1);
else
dJointAddSliderForce (joint->id(),1);
break;
case 'y':
case 'Y':
if (joint->getType() == dJointTypePiston)
dJointAddPistonForce (joint->id(),-1);
else
dJointAddSliderForce (joint->id(),-1);
break;
case '8' :
dJointAttach(joint->id(), body[0], 0);
break;
case '9' :
dJointAttach(joint->id(), 0, body[0]);
break;
case 'i':
case 'I' :
joint->setParam (dParamLoStop, 0);
joint->setParam (dParamHiStop, 0);
break;
case 'o':
case 'O' :
joint->setParam (dParamLoStop2, 0);
joint->setParam (dParamHiStop2, 0);
break;
case 'k':
case 'K':
joint->setParam (dParamLoStop2, -45.0*3.14159267/180.0);
joint->setParam (dParamHiStop2, 45.0*3.14159267/180.0);
break;
case 'l':
case 'L':
joint->setParam (dParamLoStop2, -dInfinity);
joint->setParam (dParamHiStop2, dInfinity);
break;
// Velocity of joint
case ',':
case '<' :
{
dReal vel = joint->getParam (dParamVel) - VEL_INC;
joint->setParam (dParamVel, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case '.':
case '>' :
{
dReal vel = joint->getParam (dParamVel) + VEL_INC;
joint->setParam (dParamVel, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case 'p' :
case 'P' :
{
switch (joint->getType() )
{
case dJointTypeSlider :
{
dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
std::cout<<"Position ="<<sj->getPosition() <<"\n";
}
break;
case dJointTypePiston :
{
dPistonJoint *rj = reinterpret_cast<dPistonJoint *> (joint);
std::cout<<"Position ="<<rj->getPosition() <<"\n";
}
break;
default:
{} // keep the compiler happy
}
}
break;
case '+' :
(++tc) %= 4;
setPositionBodies (tc);
break;
case '-' :
(--tc) %= 4;
setPositionBodies (tc);
break;
}
}
static void drawBox (dGeomID id, int R, int G, int B)
{
if (!id)
return;
const dReal *pos = dGeomGetPosition (id);
const dReal *rot = dGeomGetRotation (id);
dsSetColor (R,G,B);
dVector3 l;
dGeomBoxGetLengths (id, l);
dsDrawBox (pos, rot, l);
}
// simulation loop
static void simLoop (int pause)
{
const dReal *rot;
dVector3 ax;
dReal l=0;
switch (joint->getType() )
{
case dJointTypeSlider :
( (dSliderJoint *) joint)->getAxis (ax);
l = ( (dSliderJoint *) joint)->getPosition();
break;
case dJointTypePiston :
( (dPistonJoint *) joint)->getAxis (ax);
l = ( (dPistonJoint *) joint)->getPosition();
break;
default:
{} // keep the compiler happy
}
if (!pause)
{
double simstep = 0.01; // 1ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[BODY2], 1,1,0);
drawBox (geom[RECT], 0,0,1);
if ( geom[BODY1] )
{
const dReal *pos = dGeomGetPosition (geom[BODY1]);
rot = dGeomGetRotation (geom[BODY1]);
dsSetColor (0,0,1);
dGeomCapsuleGetParams (geom[BODY1], &radius, &length);
dsDrawCapsule (pos, rot, length, radius);
}
drawBox (geom[OBS], 1,0,1);
// Draw the prismatic axis
if ( geom[BODY1] )
{
const dReal *pos = dGeomGetPosition (geom[BODY1]);
rot = dGeomGetRotation (geom[BODY2]);
dVector3 p;
p[X] = pos[X] - l*ax[X];
p[Y] = pos[Y] - l*ax[Y];
p[Z] = pos[Z] - l*ax[Z];
dsSetColor (1,0,0);
dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS);
}
if (joint->getType() == dJointTypePiston )
{
dVector3 anchor;
dJointGetPistonAnchor(joint->id(), anchor);
// Draw the rotoide axis
rot = dGeomGetRotation (geom[BODY2]);
dsSetColor (1,0.5,0);
dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS);
dsSetColor (0,1,1);
rot = dGeomGetRotation (geom[BODY1]);
dsDrawSphere (anchor, rot, 1.5*RADIUS);
}
}
}
void Help (char **argv)
{
printf ("%s ", argv[0]);
printf (" -h | --help : print this help\n");
printf (" -s | --slider : Set the joint as a slider\n");
printf (" -p | --piston : Set the joint as a Piston. (Default joint)\n");
printf (" -1 | --offset1 : Create an offset between the 2 bodies\n");
printf (" Offset one of the body by z=-0.5 and keep the anchor\n");
printf (" point in the middle of the fixed body\n");
printf (" -2 | --offset2 : Create an offset between the 2 bodies\n");
printf (" Offset one of the body by z=-0.5 and set the anchor\n");
printf (" point in the middle of the movable body\n");
printf (" -3 | --offset3 : Create an offset between the 2 bodies\n");
printf (" Offset one of the body by z=-0.5 and set the anchor\n");
printf (" point in the middle of the 2 bodies\n");
printf (" -t | --texture-path path : Path to the texture.\n");
printf (" Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
printf (" -n | --notFixed : In free space with no gravity mode");
printf ("-notex : Don't use texture\n");
printf ("-noshadow : No shadow\n");
printf ("-noshadows : No shadows\n");
printf ("-pause : Initial pause\n");
printf ("--------------------------------------------------\n");
printf ("Hit any key to continue:");
getchar();
exit (0);
}
int main (int argc, char **argv)
{
dInitODE2(0);
bool fixed = true;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dVector3 offset;
dSetZero (offset, 4);
// Default test case
if (argc >= 2 )
{
for (int i=1; i < argc; ++i)
{
//static int tata = 0;
if (1)
{
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
type = dJointTypeSlider;
if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
{
int j = i+1;
if ( j >= argc || // Check if we have enough arguments
argv[j][0] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
tc = 1;
if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
tc = 2;
if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
tc = 3;
if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
fixed = false;
}
}
world = dWorldCreate();
dWorldSetERP (world, 0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
dMatrix3 R;
// Create the Obstacle
geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
dGeomSetCollideBits (geom[OBS], catBits[ALL]);
//Rotation of 45deg around y
dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
dGeomSetRotation (geom[OBS], R);
dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);
//Rotation of 90deg around y
// Will orient the Z axis along X
dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);
// Create Body2 (Wiil be attached to the world)
body[BODY2] = dBodyCreate (world);
// Main axis of cylinder is along X=1
dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dMassAdjust (&m, Mass1);
geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dGeomSetBody (geom[BODY2], body[BODY2]);
dGeomSetOffsetRotation (geom[BODY2], R);
dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY2], &m);
// Create Body 1 (Slider on the prismatic axis)
body[BODY1] = dBodyCreate (world);
// Main axis of capsule is along X=1
dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
dMassAdjust (&m, Mass1);
geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
dGeomSetBody (geom[BODY1], body[BODY1]);
dGeomSetOffsetRotation (geom[BODY1], R);
dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);
dMass mRect;
dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dMassAdd (&m, &mRect);
// TODO: translate m?
geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dGeomSetBody (geom[RECT], body[BODY1]);
dGeomSetOffsetPosition (geom[RECT],
(BODY1_LENGTH-RECT_SIDES[0]) /2.0,
0.0,
-RADIUS -RECT_SIDES[2]/2.0);
dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY1], &m);
setPositionBodies (tc);
if ( fixed )
{
// Attache external cylinder to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[BODY2]);
dJointSetFixed (fixed );
dWorldSetGravity (world,0,0,-0.8);
}
else
{
dWorldSetGravity (world,0,0,0);
}
// The static is here only to help debugging
switch (type)
{
case dJointTypeSlider :
{
dSliderJoint *sj = new dSliderJoint (world, 0);
sj->attach (body[BODY1], body[BODY2]);
sj->setAxis (1, 0, 0);
joint = sj;
}
break;
case dJointTypePiston : // fall through default
default:
{
dPistonJoint *pj = new dPistonJoint (world, 0);
pj->attach (body[BODY1], body[BODY2]);
pj->setAxis (1, 0, 0);
dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);
joint = pj;
}
break;
};
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

304
thirdparty/ode-0.16.5/ode/demo/demo_plane2d.cpp vendored Normal file
View File

@@ -0,0 +1,304 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// Test my Plane2D constraint.
// Uses ode-0.35 collision API.
# include <stdio.h>
# include <stdlib.h>
# include <math.h>
# include <ode/ode.h>
# include <drawstuff/drawstuff.h>
#include "texturepath.h"
# define drand48() ((double) (((double) rand()) / ((double) RAND_MAX)))
# define N_BODIES 40
# define STAGE_SIZE 8.0 // in m
# define TIME_STEP 0.01
# define K_SPRING 10.0
# define K_DAMP 10.0
//using namespace ode;
struct GlobalVars
{
dWorld dyn_world;
dBody dyn_bodies[N_BODIES];
dReal bodies_sides[N_BODIES][3];
dSpaceID coll_space_id;
dJointID plane2d_joint_ids[N_BODIES];
dJointGroup coll_contacts;
};
static GlobalVars *g_globals_ptr = NULL;
static void cb_start ()
/*************************/
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = { 0.5f*STAGE_SIZE, 0.5f*STAGE_SIZE, 0.65f*STAGE_SIZE};
static float hpr[3] = { 90.0f, -90.0f, 0 };
dsSetViewpoint (xyz, hpr);
}
static void cb_near_collision (void *, dGeomID o1, dGeomID o2)
/********************************************************************/
{
dBodyID b1 = dGeomGetBody (o1);
dBodyID b2 = dGeomGetBody (o2);
dContact contact;
// exit without doing anything if the two bodies are static
if (b1 == 0 && b2 == 0)
return;
// exit without doing anything if the two bodies are connected by a joint
if (b1 && b2 && dAreConnected (b1, b2))
{
/* MTRAP; */
return;
}
contact.surface.mode = 0;
contact.surface.mu = 0; // frictionless
if (dCollide (o1, o2, 1, &contact.geom, sizeof (dContactGeom)))
{
dJointID c = dJointCreateContact (g_globals_ptr->dyn_world.id(),
g_globals_ptr->coll_contacts.id (), &contact);
dJointAttach (c, b1, b2);
}
}
static void track_to_pos (dBody &body, dJointID joint_id,
dReal target_x, dReal target_y)
/************************************************************************/
{
dReal curr_x = body.getPosition()[0];
dReal curr_y = body.getPosition()[1];
dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x));
dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y));
}
static void cb_sim_step (int pause)
/*************************************/
{
if (! pause)
{
static dReal angle = 0;
angle += REAL( 0.01 );
track_to_pos (g_globals_ptr->dyn_bodies[0], g_globals_ptr->plane2d_joint_ids[0],
dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle) ),
dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle) ));
/* double f0 = 0.001; */
/* for (int b = 0; b < N_BODIES; b ++) */
/* { */
/* double p = 1 + b / (double) N_BODIES; */
/* double q = 2 - b / (double) N_BODIES; */
/* g_globals_ptr->dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */
/* } */
/* g_globals_ptr->dyn_bodies[0].addTorque (0, 0, 0.1); */
const int n = 10;
for (int i = 0; i < n; i ++)
{
dSpaceCollide (g_globals_ptr->coll_space_id, 0, &cb_near_collision);
g_globals_ptr->dyn_world.step (dReal(TIME_STEP/n));
g_globals_ptr->coll_contacts.empty ();
}
}
# if 1 /* [ */
{
// @@@ hack Plane2D constraint error reduction here:
for (int b = 0; b < N_BODIES; b ++)
{
const dReal *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id());
const dReal *quat_ptr;
dReal quat[4],
quat_len;
quat_ptr = dBodyGetQuaternion (g_globals_ptr->dyn_bodies[b].id());
quat[0] = quat_ptr[0];
quat[1] = 0;
quat[2] = 0;
quat[3] = quat_ptr[3];
quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]);
quat[0] /= quat_len;
quat[3] /= quat_len;
dBodySetQuaternion (g_globals_ptr->dyn_bodies[b].id(), quat);
dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(), 0, 0, rot[2]);
}
}
# endif /* ] */
# if 0 /* [ */
{
// @@@ friction
for (int b = 0; b < N_BODIES; b ++)
{
const dReal *vel = dBodyGetLinearVel (g_globals_ptr->dyn_bodies[b].id()),
*rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id());
dReal s = 1.00;
dReal t = 0.99;
dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]);
dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]);
}
}
# endif /* ] */
{
// ode drawstuff
dsSetTexture (DS_WOOD);
for (int b = 0; b < N_BODIES; b ++)
{
if (b == 0)
dsSetColor (1.0, 0.5, 1.0);
else
dsSetColor (0, 0.5, 1.0);
#ifdef dDOUBLE
dsDrawBoxD (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]);
#else
dsDrawBox (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]);
#endif
}
}
}
extern int main
/******************/
(
int argc,
char **argv
)
{
int b;
dsFunctions drawstuff_functions;
dInitODE2(0);
g_globals_ptr = new GlobalVars();
// dynamic world
dReal cf_mixing;// = 1 / TIME_STEP * K_SPRING + K_DAMP;
dReal err_reduct;// = TIME_STEP * K_SPRING * cf_mixing;
err_reduct = REAL( 0.5 );
cf_mixing = REAL( 0.001 );
dWorldSetERP (g_globals_ptr->dyn_world.id (), err_reduct);
dWorldSetCFM (g_globals_ptr->dyn_world.id (), cf_mixing);
g_globals_ptr->dyn_world.setGravity (0, 0.0, -1.0);
g_globals_ptr->coll_space_id = dSimpleSpaceCreate (0);
// dynamic bodies
for (b = 0; b < N_BODIES; b ++)
{
int l = (int) (1 + sqrt ((double) N_BODIES));
dReal x = dReal( (0.5 + (b / l)) / l * STAGE_SIZE );
dReal y = dReal( (0.5 + (b % l)) / l * STAGE_SIZE );
dReal z = REAL( 1.0 ) + REAL( 0.1 ) * (dReal)drand48();
g_globals_ptr->bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) );
g_globals_ptr->bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) );
g_globals_ptr->bodies_sides[b][2] = z;
g_globals_ptr->dyn_bodies[b].create (g_globals_ptr->dyn_world);
g_globals_ptr->dyn_bodies[b].setPosition (x, y, z/2);
g_globals_ptr->dyn_bodies[b].setData ((void*) (dsizeint)b);
dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id (),
dReal( 3 * (drand48 () - 0.5) ),
dReal( 3 * (drand48 () - 0.5) ), 0);
dMass m;
m.setBox (1, g_globals_ptr->bodies_sides[b][0],g_globals_ptr->bodies_sides[b][1],g_globals_ptr->bodies_sides[b][2]);
m.adjust (REAL(0.1) * g_globals_ptr->bodies_sides[b][0] * g_globals_ptr->bodies_sides[b][1]);
g_globals_ptr->dyn_bodies[b].setMass (&m);
g_globals_ptr->plane2d_joint_ids[b] = dJointCreatePlane2D (g_globals_ptr->dyn_world.id (), 0);
dJointAttach (g_globals_ptr->plane2d_joint_ids[b], g_globals_ptr->dyn_bodies[b].id (), 0);
}
dJointSetPlane2DXParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10);
dJointSetPlane2DYParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10);
// collision geoms and joints
dCreatePlane (g_globals_ptr->coll_space_id, 1, 0, 0, 0);
dCreatePlane (g_globals_ptr->coll_space_id, -1, 0, 0, -STAGE_SIZE);
dCreatePlane (g_globals_ptr->coll_space_id, 0, 1, 0, 0);
dCreatePlane (g_globals_ptr->coll_space_id, 0, -1, 0, -STAGE_SIZE);
for (b = 0; b < N_BODIES; b ++)
{
dGeomID coll_box_id;
coll_box_id = dCreateBox (g_globals_ptr->coll_space_id,
g_globals_ptr->bodies_sides[b][0], g_globals_ptr->bodies_sides[b][1], g_globals_ptr->bodies_sides[b][2]);
dGeomSetBody (coll_box_id, g_globals_ptr->dyn_bodies[b].id ());
}
g_globals_ptr->coll_contacts.create ();
{
// simulation loop (by drawstuff lib)
drawstuff_functions.version = DS_VERSION;
drawstuff_functions.start = &cb_start;
drawstuff_functions.step = &cb_sim_step;
drawstuff_functions.command = 0;
drawstuff_functions.stop = 0;
drawstuff_functions.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dsSimulationLoop (argc, argv, 352,288,&drawstuff_functions);
}
delete g_globals_ptr;
g_globals_ptr = NULL;
dCloseODE();
return 0;
}

258
thirdparty/ode-0.16.5/ode/demo/demo_rfriction.cpp vendored Normal file
View File

@@ -0,0 +1,258 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
Angular friction demo:
A bunch of ramps of different pitch.
A bunch of spheres with rolling friction.
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define GRAVITY 10 // the global gravity to use
#define RAMP_COUNT 8
static const dReal rampX = 6.0f;
static const dReal rampY = 0.5f;
static const dReal rampZ = 0.25f;
static const dReal sphereRadius = 0.25f;
static const dReal maxRamp = M_PI/4.0f; // Needs to be less than pi/2
static const dReal rampInc = maxRamp/RAMP_COUNT;
// dynamics and collision objects
static dWorldID world = 0;
static dSpaceID space = 0;
static dJointGroupID contactgroup = 0;
static dGeomID ground;
static dReal mu = REAL(0.37); // the global mu to use
static dReal rho = REAL(0.1); // the global rho to use
static dReal omega = REAL(25.0);
static dGeomID rampGeom[RAMP_COUNT];
static dBodyID sphereBody[RAMP_COUNT];
static dGeomID sphereGeom[RAMP_COUNT];
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1==0 && b2==0) return;
dContact contact[3];
for (int ii=0; ii<3; ii++) {
contact[ii].surface.mode = dContactApprox1 | dContactRolling;
contact[ii].surface.mu = mu;
contact[ii].surface.rho = rho;
}
if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {0,-3.0f,3.0f};
static float hpr[3] = {90.0000,-15.0000,0.0000};
dsSetViewpoint (xyz,hpr);
printf ("Press:\n"
"\t'[' or ']' to change initial angular velocity\n"
"\t'm' to increase sliding friction\n"
"\t'n' to decrease sliding friction\n"
"\t'j' to increase rolling friction\n"
"\t'h' to decrease rolling friction\n"
"\t'r' to reset simulation.\n");
}
/**
Delete the bodies, etc.
*/
static void clear()
{
if (contactgroup) dJointGroupDestroy (contactgroup);
if (space) dSpaceDestroy (space);
if (world) dWorldDestroy (world);
}
/**
Cleanup if necessary and rebuild the
world.
*/
static void reset()
{
clear();
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-GRAVITY);
ground = dCreatePlane (space,0,0,1,0);
// Calculate mass for sphere a capsule with water density.
dMass sphereMass;
dMassSetSphere(&sphereMass,1000,sphereRadius);
for (int ii=0;ii<RAMP_COUNT;++ii) {
dQuaternion q;
dReal angle = (ii+1)*rampInc;
dReal cosA = dCos(angle);
dReal sinA = dSin(angle);
dReal rampW = rampX/cosA; // Box width that preserves ground distance
dReal zPos = REAL(0.5)*(sinA*rampW-cosA*rampZ); // Position that makes end meet ground
dReal yPos = ii*1.25*rampY;
dReal xPos = 0;
// Create the ramp
rampGeom[ii] = dCreateBox(space,rampW,rampY,rampZ);
dQFromAxisAndAngle(q,0,1,0,angle);
dGeomSetQuaternion(rampGeom[ii],q);
dGeomSetPosition(rampGeom[ii],xPos,yPos,zPos);
// Create the spheres
xPos = -REAL(0.5)*rampX + sphereRadius;
zPos = sinA*rampW + sphereRadius;
sphereBody[ii] = dBodyCreate(world);
dBodySetMass(sphereBody[ii],&sphereMass);
sphereGeom[ii] = dCreateSphere(space,sphereRadius);
dGeomSetBody(sphereGeom[ii],sphereBody[ii]);
dBodySetPosition(sphereBody[ii],xPos,yPos,zPos);
dBodySetAngularVel(sphereBody[ii],0,omega,0);
}
}
static void command (int cmd)
{
switch (cmd) {
case 'h': case 'H':
rho-=0.02;
if (rho<0) rho=0;
break;
case 'j': case 'J':
rho+=0.02;
if (rho>1) rho=1;
break;
case 'n': case 'N':
mu-=0.02;
if (mu<0) mu=0;
break;
case 'm': case 'M':
mu+=0.02;
if (mu>1) mu=1;
break;
case 'r': case 'R':
reset();
break;
case ']':
omega+=1;
break;
case '[':
omega-=1;
break;
}
}
// simulation loop
static void simLoop (int pause)
{
if (!pause) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.017); // 60 fps
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
// Render ramps and spheres
dsSetTexture (DS_WOOD);
for (int ii=0;ii<RAMP_COUNT;++ii) {
dVector3 sides;
dsSetColor (1,0.5,0);
dGeomBoxGetLengths(rampGeom[ii],sides);
dsDrawBox (dGeomGetPosition(rampGeom[ii]),dGeomGetRotation(rampGeom[ii]),sides);
dsSetColor(0,0,1);
dsDrawSphere (dGeomGetPosition(sphereGeom[ii]),dGeomGetRotation(sphereGeom[ii]), sphereRadius);
}
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dInitODE2(0);
reset();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
clear();
dCloseODE();
return 0;
}

171
thirdparty/ode-0.16.5/ode/demo/demo_slider.cpp vendored Normal file
View File

@@ -0,0 +1,171 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#endif
// some constants
#define SIDE (0.5f) // side length of a box
#define MASS (1.0) // mass of a box
// dynamics and collision objects
static dWorldID world;
static dBodyID body[2];
static dJointID slider;
// state set by keyboard commands
static int occasional_error = 0;
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press 'e' to start/stop occasional error.\n");
}
// called when a key pressed
static void command (int cmd)
{
if (cmd == 'e' || cmd == 'E') {
occasional_error ^= 1;
}
}
// simulation loop
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
const dReal ks = 0.5; // spring constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
a += 0.01;
// add a spring force to keep the bodies together, otherwise they will
// fly apart along the slider axis.
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
ks*(p2[2]-p1[2]));
dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
ks*(p1[2]-p2[2]));
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
dWorldStep (world,0.05);
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0,0,1);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dQuaternion q;
dQFromAxisAndAngle (q,-1,1,0,0.25*M_PI);
dBodySetPosition (body[1],0.2,0.2,1.2);
dBodySetQuaternion (body[1],q);
slider = dJointCreateSlider (world,0);
dJointAttach (slider,body[0],body[1]);
dJointSetSliderAxis (slider,1,1,1);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}

232
thirdparty/ode-0.16.5/ode/demo/demo_space.cpp vendored Normal file
View File

@@ -0,0 +1,232 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
testing procedure:
* create a bunch of random boxes
* test for intersections directly, put results in n^2 array
* get space to report collisions:
- all correct collisions reported
- no pair reported more than once
- no incorrect collisions reported
*/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define NUM 20 // number of boxes to test
// collision objects and globals
static dSpaceID space;
static dGeomID geom[NUM];
static dReal bounds[NUM][6];
static dsizeint good_matrix[NUM][NUM]; // correct collision matrix
static dsizeint test_matrix[NUM][NUM]; // testing collision matrix
static dsizeint hits[NUM]; // number of collisions a box has
static unsigned long seed=37;
static void init_test()
{
int i,j;
const dReal scale = 0.5;
// set random boxes
dRandSetSeed (seed);
for (i=0; i < NUM; i++) {
bounds[i][0] = dRandReal()*2-1;
bounds[i][1] = bounds[i][0] + dRandReal()*scale;
bounds[i][2] = dRandReal()*2-1;
bounds[i][3] = bounds[i][2] + dRandReal()*scale;
bounds[i][4] = dRandReal()*2;
bounds[i][5] = bounds[i][4] + dRandReal()*scale;
if (geom[i]) dGeomDestroy (geom[i]);
geom[i] = dCreateBox (space,
bounds[i][1] - bounds[i][0],
bounds[i][3] - bounds[i][2],
bounds[i][5] - bounds[i][4]);
dGeomSetPosition (geom[i],
(bounds[i][0] + bounds[i][1])*0.5,
(bounds[i][2] + bounds[i][3])*0.5,
(bounds[i][4] + bounds[i][5])*0.5);
dGeomSetData (geom[i],(void*)(dsizeint)(i));
}
// compute all intersections and put the results in "good_matrix"
for (i=0; i < NUM; i++) {
for (j=0; j < NUM; j++) good_matrix[i][j] = 0;
}
for (i=0; i < NUM; i++) hits[i] = 0;
for (i=0; i < NUM; i++) {
for (j=i+1; j < NUM; j++) {
dReal *bounds1 = &bounds[i][0];
dReal *bounds2 = &bounds[j][0];
if (bounds1[0] > bounds2[1] ||
bounds1[1] < bounds2[0] ||
bounds1[2] > bounds2[3] ||
bounds1[3] < bounds2[2] ||
bounds1[4] > bounds2[5] ||
bounds1[5] < bounds2[4]) continue;
good_matrix[i][j] = 1;
good_matrix[j][i] = 1;
hits[i]++;
hits[j]++;
}
}
}
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
dsizeint i,j;
i = (dsizeint) dGeomGetData (o1);
j = (dsizeint) dGeomGetData (o2);
if (i==j)
printf ("collision (%d,%d) is between the same object\n",(int)i,(int)j);
if (!good_matrix[i][j] || !good_matrix[j][i])
printf ("collision (%d,%d) is incorrect\n",(int)i,(int)j);
if (test_matrix[i][j] || test_matrix[j][i])
printf ("collision (%d,%d) reported more than once\n",(int)i,(int)j);
test_matrix[i][j] = 1;
test_matrix[j][i] = 1;
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
static void command (int cmd)
{
if (cmd == ' ') {
seed++;
init_test();
}
}
// simulation loop
static void simLoop (int)
{
int i,j;
for (i=0; i < NUM; i++) {
for (j=0; j < NUM; j++) test_matrix[i][j] = 0;
}
dSpaceCollide (space,0,&nearCallback);
for (i=0; i < NUM; i++) {
for (j=i+1; j < NUM; j++) {
if (good_matrix[i][j] && !test_matrix[i][j]) {
printf ("failed to report collision (%d,%d) (seed=%ld)\n",i,j,seed);
}
}
}
seed++;
init_test();
for (i=0; i<NUM; i++) {
dVector3 pos,side;
dMatrix3 R;
dRSetIdentity (R);
for (j=0; j<3; j++) pos[j] = (bounds[i][j*2+1] + bounds[i][j*2]) * 0.5;
for (j=0; j<3; j++) side[j] = bounds[i][j*2+1] - bounds[i][j*2];
if (hits[i] > 0) dsSetColor (1,0,0);
else dsSetColor (1,1,0);
dsDrawBox (pos,R,side);
}
}
int main (int argc, char **argv)
{
int i;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dInitODE2(0);
// test the simple space:
// space = dSimpleSpaceCreate();
// test the hash space:
// space = dHashSpaceCreate (0);
// dHashSpaceSetLevels (space,-10,10);
// test the quadtree space
dVector3 Center = {0, 0, 0, 0};
dVector3 Extents = {10, 0, 10, 0};
space = dQuadTreeSpaceCreate(0, Center, Extents, 7);
for (i=0; i < NUM; i++) geom[i] = 0;
init_test();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dSpaceDestroy (space);
dCloseODE();
return 0;
}

449
thirdparty/ode-0.16.5/ode/demo/demo_space_stress.cpp vendored Normal file
View File

@@ -0,0 +1,449 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <string>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define NUM 10000 // max number of objects
#define DENSITY (5.0) // density of all objects
#define GPB 3 // maximum number of geometries per body
#define MAX_CONTACTS 4 // maximum number of contact points per body
#define WORLD_SIZE 20
#define WORLD_HEIGHT 20
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom[GPB]; // geometries representing this body
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space = NULL;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1; // selected object
static int show_aabb = 0; // show geom AABBs?
static int show_contacts = 0; // show contact points?
static int random_pos = 1; // drop objects from random position?
static int draw_geom = 1;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i<MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.1640f,-1.3079f,3.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("To drop another object, press:\n");
printf (" o to disable rendering.\n");
printf (" b for box.\n");
printf (" s for sphere.\n");
printf (" c for cylinder.\n");
printf (" x for a composite object.\n");
printf (" y for cylinder.\n");
printf ("To select an object, press space.\n");
printf ("To disable the selected object, press d.\n");
printf ("To enable the selected object, press e.\n");
printf ("To toggle showing the geom AABBs, press a.\n");
printf ("To toggle showing the contact points, press t.\n");
printf ("To toggle dropping from random position/orientation, press r.\n");
}
char locase(char c)
{
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase(cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y') {
if (num < NUM) {
// new object to be created
i = num;
num++;
} else {
// recycle existing object
i = nextobj++;
nextobj %= num; // wrap-around if needed
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
obj[i].body = 0;
for (k=0; k < GPB; k++)
if (obj[i].geom[k]) {
dGeomDestroy(obj[i].geom[k]);
obj[i].geom[k] = 0;
}
}
obj[i].body = dBodyCreate(world);
for (k=0; k<3; k++)
sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition(obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle(R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
} else {
// higher than highest body position
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition(obj[k].body);
if (pos[2] > maxheight)
maxheight = pos[2];
}
dBodySetPosition(obj[i].body, 0,0,maxheight+1);
dRSetIdentity(R);
//dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
}
dBodySetRotation(obj[i].body,R);
if (cmd == 'b') {
dMassSetBox(&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox(space,sides[0],sides[1],sides[2]);
} else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule(&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
} else if (cmd == 'y') {
dMassSetCylinder(&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder(space,sides[0],sides[1]);
} else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
} else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == 'o') {
draw_geom ^= 1;
}
}
// draw a geom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!draw_geom){
return;
}
if (!g) return;
if (!pos) pos = dGeomGetPosition(g);
if (!R) R = dGeomGetRotation(g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths(g,sides);
dsDrawBox(pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere(pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams(g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
} else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams(g,&radius,&length);
dsDrawCylinder(pos,R,length,radius);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB(g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++)
bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++)
bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity(RI);
dsSetColorAlpha(1,0,0,0.5);
dsDrawBox(bbpos,RI,bbsides);
}
}
// simulation loop
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
//if (!pause) dWorldStep (world,0.05);
if (!pause) dWorldQuickStep (world,0.05);
//if (!pause) dWorldStepFast (world,0.05, 1);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dsSetSphereQuality(0);
// create world
dInitODE2(0);
world = dWorldCreate();
for (int i=1; i<argc; ++i) {
if (argv[i] == std::string("quad")) {
dVector3 Center = {0, 0, 0, 0};
dVector3 Extents = {WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, WORLD_SIZE * 0.55, 0};
puts(":::: Using dQuadTreeSpace");
space = dQuadTreeSpaceCreate (0, Center, Extents, 6);
} else if (argv[i] == std::string("hash")) {
puts(":::: Using dHashSpace");
space = dHashSpaceCreate (0);
} else if (argv[i] == std::string("sap")) {
puts(":::: Using dSweepAndPruneSpace");
space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
} else if (argv[i] == std::string("simple")) {
puts(":::: Using dSimpleSpace");
space = dSimpleSpaceCreate(0);
}
}
if (!space) {
puts(":::: You can specify 'quad', 'hash', 'sap' or 'simple' in the");
puts(":::: command line to specify the type of space.");
puts(":::: Using SAP space by default.");
space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
}
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
for (int i = 0; i < NUM; i++){
command('s');
}
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

192
thirdparty/ode-0.16.5/ode/demo/demo_step.cpp vendored Normal file
View File

@@ -0,0 +1,192 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// test the step function by comparing the output of the fast and the slow
// version, for various systems. currently you have to define COMPARE_METHODS
// in step.cpp for this to work properly.
//
// @@@ report MAX error
#include <time.h>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
// some constants
#define NUM 10 // number of bodies
#define NUMJ 9 // number of joints
#define SIDE (0.2) // side length of a box
#define MASS (1.0) // mass of a box
#define RADIUS (0.1732f) // sphere radius
// dynamics and collision objects
static dWorldID world=0;
static dBodyID body[NUM];
static dJointID joint[NUMJ];
// create the test system
void createTest()
{
int i,j;
if (world) dWorldDestroy (world);
world = dWorldCreate();
// create random bodies
for (i=0; i<NUM; i++) {
// create bodies at random position and orientation
body[i] = dBodyCreate (world);
dBodySetPosition (body[i],dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2+RADIUS);
dReal q[4];
for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
dBodySetQuaternion (body[i],q);
// set random velocity
dBodySetLinearVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1);
dBodySetAngularVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1);
// set random mass (random diagonal mass rotated by a random amount)
dMass m;
dMatrix3 R;
dMassSetBox (&m,1,dRandReal()+0.1,dRandReal()+0.1,dRandReal()+0.1);
dMassAdjust (&m,dRandReal()+1);
for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
dQtoR (q,R);
dMassRotate (&m,R);
dBodySetMass (body[i],&m);
}
// create ball-n-socket joints at random positions, linking random bodies
// (but make sure not to link the same pair of bodies twice)
char linked[NUM*NUM];
for (i=0; i<NUM*NUM; i++) linked[i] = 0;
for (i=0; i<NUMJ; i++) {
int b1,b2;
do {
b1 = dRandInt (NUM);
b2 = dRandInt (NUM);
} while (linked[b1*NUM + b2] || b1==b2);
linked[b1*NUM + b2] = 1;
linked[b2*NUM + b1] = 1;
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[b1],body[b2]);
dJointSetBallAnchor (joint[i],dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*2+RADIUS);
}
for (i=0; i<NUM; i++) {
// move bodies a bit to get some joint error
const dReal *pos = dBodyGetPosition (body[i]);
dBodySetPosition (body[i],pos[0]+dRandReal()*0.2-0.1,
pos[1]+dRandReal()*0.2-0.1,pos[2]+dRandReal()*0.2-0.1);
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.6117f,-1.4433f,2.3700f};
static float hpr[3] = {151.5000f,-30.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
}
// simulation loop
static void simLoop (int pause)
{
if (!pause) {
// add random forces and torques to all bodies
int i;
const dReal scale1 = 5;
const dReal scale2 = 5;
for (i=0; i<NUM; i++) {
dBodyAddForce (body[i],
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1));
dBodyAddTorque (body[i],
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1));
}
dWorldStep (world,0.05);
createTest();
}
// float sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS);
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dInitODE2(0);
dRandSetSeed (time(0));
createTest();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}

498
thirdparty/ode-0.16.5/ode/demo/demo_tracks.cpp vendored Normal file
View File

@@ -0,0 +1,498 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
//#include <iostream>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#define dsDrawBox dsDrawBoxD
#define dsDrawTriangle dsDrawTriangleD
#define dsDrawLine dsDrawLineD
#endif
const dReal ball_radius = 0.4;
const dReal balls_sep = 2; // separation between the balls
/* Choose one test case
*/
#define TEST_CASE 0
#if TEST_CASE == 0
const dReal track_len = 10;
const dReal track_height = 1;
const dReal track_width = 0.1;
const dReal track_gauge = 1;
const dReal track_elevation = 2;
const dReal track_angle = 80 * M_PI/180.;
const dReal track_incl = 10 * M_PI/180.;
#elif TEST_CASE == 1
const dReal track_len = 10;
const dReal track_height = 1;
const dReal track_width = 0.1;
const dReal track_gauge = 1.9*ball_radius;
const dReal track_elevation = 2;
const dReal track_angle = 0 * M_PI/180.;
const dReal track_incl = 10 * M_PI/180.;
#elif TEST_CASE == 2
const dReal track_len = 10;
const dReal track_height = 1;
const dReal track_width = 0.1;
const dReal track_gauge = 1.9*ball_radius;
const dReal track_elevation = 2;
const dReal track_angle = 15 * M_PI/180.;
const dReal track_incl = 10 * M_PI/180.;
#elif TEST_CASE == 3
const dReal track_len = 10;
const dReal track_height = .7;
const dReal track_width = 0.1;
const dReal track_gauge = track_height*1.1;
const dReal track_elevation = 2;
const dReal track_angle = 90 * M_PI/180.;
const dReal track_incl = 10 * M_PI/180.;
#else
#error "TEST_CAST to a valid value!"
#endif
dWorldID world;
dSpaceID space;
dJointGroupID contact_group;
dGeomID ground;
dGeomID ball1_geom, ball2_geom;
dTriMeshDataID mesh_data;
dGeomID mesh_geom;
dBodyID ball1_body, ball2_body;
const unsigned n_box_verts = 8;
dVector3 box_verts[n_box_verts] = {
{-track_len/2, -track_width/2, track_height/2}, // 0
{ track_len/2, -track_width/2, track_height/2}, // 1
{ track_len/2, track_width/2, track_height/2}, // 2
{-track_len/2, track_width/2, track_height/2}, // 3
{ track_len/2, -track_width/2, -track_height/2}, // 4
{-track_len/2, -track_width/2, -track_height/2}, // 5
{-track_len/2, track_width/2, -track_height/2}, // 6
{ track_len/2, track_width/2, -track_height/2} // 7
};
const unsigned n_box_faces = 12;
dTriIndex box_faces[n_box_faces * 3] = {
0, 1, 2,
0, 2, 3,
1, 4, 7,
1, 7, 2,
4, 5, 6,
4, 6, 7,
5, 0, 3,
5, 3, 6,
3, 2, 7,
3, 7, 6,
0, 5, 4,
0, 4, 1
};
const unsigned n_track_verts = n_box_verts * 2;
const unsigned n_track_faces = n_box_faces * 2;
dVector3 track_verts[n_track_verts];
dTriIndex track_faces[n_track_faces * 3];
void resetBall(dBodyID b, unsigned idx)
{
dBodySetPosition(b,
0.5*track_len*cos(track_incl) // Z
- 0.5*track_height*sin(track_incl)
- ball_radius, // X
balls_sep*idx, // Y
track_elevation + ball_radius// Z
+ 0.5*track_len*sin(track_incl)
+ 0.5*track_height*cos(track_incl));
dMatrix3 r = {1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0};
dBodySetRotation(b, r);
dBodySetLinearVel(b, 0, 0, 0);
dBodySetAngularVel(b, 0, 0, 0);
}
void resetSim()
{
resetBall(ball1_body, 0);
resetBall(ball2_body, 1);
}
void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
world = dWorldCreate();
dWorldSetGravity (world,0,0,-9.8);
contact_group = dJointGroupCreate(0);
space = dSimpleSpaceCreate (0);
// first, the ground plane
// it has to coincide with the plane we have in drawstuff
ground = dCreatePlane(space, 0, 0, 1, 0);
// now a ball
dMass m;
dMassSetSphere(&m, 0.1, ball_radius);
ball1_geom = dCreateSphere(space, ball_radius);
ball1_body = dBodyCreate(world);
dGeomSetBody(ball1_geom, ball1_body);
dBodySetMass(ball1_body, &m);
ball2_geom = dCreateSphere(space, ball_radius);
ball2_body = dBodyCreate(world);
dGeomSetBody(ball2_geom, ball2_body);
dBodySetMass(ball2_body, &m);
// tracks made out of boxes
dGeomID trk;
dMatrix3 r1, r2, r3;
dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation};
dMatrix3 s1, s2, s3;
dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation};
dRFromAxisAndAngle(r1, 1, 0, 0, track_angle);
dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl);
dMultiply0_333(r3, r2, r1);
dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle);
dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl);
dMultiply0_333(s3, s2, s1);
trk = dCreateBox(space, track_len, track_width, track_height);
dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]);
dGeomSetRotation(trk, r3);
trk = dCreateBox(space, track_len, track_width, track_height);
dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]);
dGeomSetRotation(trk, s3);
// tracks made out of trimesh
for (unsigned i=0; i<n_box_verts; ++i) {
dVector3 p;
dMultiply0_331(p, s3, box_verts[i]);
dAddVectors3(p, p, so);
dCopyVector3(track_verts[i], p);
}
// trimesh tracks 2, transform all vertices by s3
for (unsigned i=0; i<n_box_verts; ++i) {
dVector3 p;
dMultiply0_331(p, r3, box_verts[i]);
dAddVectors3(p, p, ro);
dCopyVector3(track_verts[n_box_verts + i], p);
}
// copy face indices
for (unsigned i=0; i<n_box_faces; ++i)
for (unsigned j=0; j<3; ++j) // each face index
track_faces[3*i+j] = box_faces[3*i+j];
for (unsigned i=0; i<n_box_faces; ++i)
for (unsigned j=0; j<3; ++j) // each face index
track_faces[3*(i + n_box_faces)+j] = box_faces[3*i+j] + n_box_verts;
mesh_data = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSimple(mesh_data,
track_verts[0], n_track_verts,
track_faces, 3*n_track_faces);
mesh_geom = dCreateTriMesh(space, mesh_data, 0, 0, 0);
resetSim();
// initial camera position
static float xyz[3] = {-5.9414,-0.4804,2.9800};
static float hpr[3] = {32.5000,-10.0000,0.0000};
dsSetViewpoint (xyz,hpr);
dsSetSphereQuality(3);
}
void nearCallback(void *, dGeomID a, dGeomID b)
{
const unsigned max_contacts = 8;
dContact contacts[max_contacts];
if (!dGeomGetBody(a) && !dGeomGetBody(b))
return; // don't handle static geom collisions
int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
//clog << "got " << n << " contacts" << endl;
/* Simple contact merging:
* If we have contacts that are too close with the same normal, keep only
* the one with maximum depth.
* The epsilon that defines what "too close" means can be a heuristic.
*/
int new_n = 0;
dReal epsilon = 1e-1; // default
/* If we know one of the geoms is a sphere, we can base the epsilon on the
* sphere's radius.
*/
dGeomID s = 0;
if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
(dGeomGetClass(b) == dSphereClass && (s = b))) {
epsilon = dGeomSphereGetRadius(s) * 0.3;
}
for (int i=0; i<n; ++i) {
// this block draws the contact points before merging, in red
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(1, 0, 0);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.008);
// let's offset the line a bit to avoid drawing overlap issues
float xyzf[3], hprf[3];
dsGetViewpoint(xyzf, hprf);
dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
dVector3 v;
dSubtractVectors3(v, contacts[i].geom.pos, xyz);
dVector3 c;
dCalcVectorCross3(c, v, contacts[i].geom.pos);
dNormalize3(c);
dVector3 pos1;
dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
dVector3 pos2;
dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
dsDrawLine(pos1, pos2);
// end of contacts drawing code
int closest_point = i;
for (int j=0; j<new_n; ++j) {
dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
if (alignment > 0.99 // about 8 degrees of difference
&&
dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
// they are too close
closest_point = j;
//clog << "found close points: " << j << " and " << i << endl;
break;
}
}
if (closest_point != i) {
// we discard one of the points
if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
// the new point is deeper, copy it over closest_point
contacts[closest_point] = contacts[i];
} else
contacts[new_n++] = contacts[i]; // the point is preserved
}
//clog << "reduced from " << n << " to " << new_n << endl;
n = new_n;
for (int i=0; i<n; ++i) {
contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
contacts[i].surface.mu = 10;
contacts[i].surface.bounce = 0.2;
contacts[i].surface.bounce_vel = 0;
contacts[i].surface.soft_erp = 1e-3;
//clog << "depth: " << contacts[i].geom.depth << endl;
dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(0, 0, 1);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.01);
dsSetColor(0, 1, 0);
dVector3 pos2;
dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
dsDrawLine(contacts[i].geom.pos, pos2);
}
//clog << "----" << endl;
}
void stop()
{
dGeomDestroy(mesh_geom);
dGeomTriMeshDataDestroy(mesh_data);
dBodyDestroy(ball1_body);
dBodyDestroy(ball2_body);
dGeomDestroy(ground);
dJointGroupDestroy(contact_group);
dSpaceDestroy(space); // will destroy all geoms
dWorldDestroy(world);
}
static void command (int cmd)
{
switch (cmd) {
case ' ':
resetSim();
break;
}
}
void drawGeom(dGeomID g)
{
int gclass = dGeomGetClass(g);
const dReal *pos = dGeomGetPosition(g);
const dReal *rot = dGeomGetRotation(g);
switch (gclass) {
case dSphereClass:
dsSetColorAlpha(0, 0.75, 0.5, 0.5);
dsSetTexture (DS_CHECKERED);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(g));
break;
case dBoxClass:
{
dVector3 lengths;
dsSetColorAlpha(1, 1, 0, 0.5);
dsSetTexture (DS_WOOD);
dGeomBoxGetLengths(g, lengths);
dsDrawBox(pos, rot, lengths);
break;
}
case dTriMeshClass:
{
int numi = dGeomTriMeshGetTriangleCount(g);
for (int i=0; i<numi; ++i) {
dVector3 v0, v1, v2;
dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2);
dsSetTexture (DS_WOOD);
dsSetDrawMode(DS_WIREFRAME);
dsSetColorAlpha(0, 0, 0, 1.0);
dsDrawTriangle(pos, rot, v0, v1, v2, true);
dsSetDrawMode(DS_POLYFILL);
dsSetColorAlpha(1, 1, 0, 0.5);
dsDrawTriangle(pos, rot, v0, v1, v2, true);
}
break;
}
default:
{}
}
}
void simLoop (int pause)
{
if (!pause) {
const dReal step = 0.02;
const unsigned nsteps = 1;
for (unsigned i=0; i<nsteps; ++i) {
dSpaceCollide(space, 0, nearCallback);
dWorldQuickStep(world, step);
dJointGroupEmpty(contact_group);
}
} else {
dSpaceCollide(space, 0, nearCallback);
dJointGroupEmpty(contact_group);
}
// now we draw everything
unsigned ngeoms = dSpaceGetNumGeoms(space);
for (unsigned i=0; i<ngeoms; ++i) {
dGeomID g = dSpaceGetGeom(space, i);
if (g == ground)
continue; // drawstuff is already drawing it for us
drawGeom(g);
}
if (dBodyGetPosition(ball1_body)[0] < -track_len)
resetSim();
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = stop;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE();
// run demo
dsSimulationLoop (argc, argv, 800, 600, &fn);
dCloseODE();
return 0;
}

414
thirdparty/ode-0.16.5/ode/demo/demo_transmission.cpp vendored Normal file
View File

@@ -0,0 +1,414 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef dDOUBLE
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawLine dsDrawLineD
#define dsDrawSphere dsDrawSphereD
#endif
dReal theta = M_PI / 4;
dReal ratio = 1, speed = 5, rho_1 = 1, rho_2 = 1, backlash = 0.1;
int mode = 0;
dWorldID world;
dSpaceID space;
dBodyID body1, body2;
dGeomID geom1, geom2;
dJointID hinge1, hinge2, transmission;
dJointFeedback feedback;
void setup() {
dMatrix3 R;
switch (mode) {
case 0:
// Parallel axes.
dBodySetPosition(body1, 1, 0, 1);
dBodySetPosition(body2, -1, 0, 1);
dRSetIdentity (R);
dBodySetRotation (body1, R);
dBodySetRotation (body2, R);
dJointSetHingeAnchor(hinge2, -1, 0, 1);
dJointSetHingeAxis(hinge2, 0, 0, 1);
dJointSetHingeAnchor(hinge1, 1, 0, 1);
dJointSetHingeAxis(hinge1, 0, 0, 1);
dJointSetTransmissionMode(transmission, dTransmissionParallelAxes);
dJointSetTransmissionRatio(transmission, ratio);
dJointSetTransmissionAnchor1(transmission, 1, 0, 1);
dJointSetTransmissionAnchor2(transmission, -1, 0, 1);
dJointSetTransmissionAxis(transmission, 0, 0, 1);
break;
case 1:
// Intersecting axes.
dBodySetPosition(body1, 1, 0, 1);
dBodySetPosition(body2, -1, 0, 2);
dRSetIdentity (R);
dBodySetRotation (body1, R);
dRFromZAxis (R, cos(theta), 0, sin(theta));
dBodySetRotation (body2, R);
dJointSetHingeAnchor(hinge2, -1, 0, 2);
dJointSetHingeAxis(hinge2, cos(theta), 0, sin(theta));
dJointSetHingeAnchor(hinge1, 1, 0, 1);
dJointSetHingeAxis(hinge1, 0, 0, 1);
dJointSetTransmissionMode(transmission, dTransmissionIntersectingAxes);
dJointSetTransmissionAnchor1(transmission, 1, 0, 1);
dJointSetTransmissionAnchor2(transmission, -1, 0, 2);
dJointSetTransmissionAxis1(transmission, 0, 0, -1);
dJointSetTransmissionAxis2(transmission, cos(theta), 0, sin(theta));
break;
case 2:
// Chain.
dBodySetPosition(body1, 2, 0, 1);
dBodySetPosition(body2, -2, 0, 1);
dRSetIdentity (R);
dBodySetRotation (body1, R);
dBodySetRotation (body2, R);
dJointSetHingeAnchor(hinge2, -2, 0, 1);
dJointSetHingeAxis(hinge2, 0, 0, 1);
dJointSetHingeAnchor(hinge1, 2, 0, 1);
dJointSetHingeAxis(hinge1, 0, 0, 1);
dJointSetTransmissionMode(transmission, dTransmissionChainDrive);
dJointSetTransmissionAnchor1(transmission, 2, 0, 1);
dJointSetTransmissionAnchor2(transmission, -2, 0, 1);
dJointSetTransmissionRadius1(transmission, rho_1);
dJointSetTransmissionRadius2(transmission, rho_2);
dJointSetTransmissionAxis(transmission, 0, 0, 1);
break;
}
dJointSetTransmissionBacklash(transmission, backlash);
dJointSetHingeParam(hinge2, dParamVel, speed);
dJointSetHingeParam(hinge2, dParamFMax, 50);
dJointSetHingeParam(hinge1, dParamVel, 0);
dJointSetHingeParam(hinge1, dParamFMax, 2);
dBodySetLinearVel(body1, 0, 0, 0);
dBodySetLinearVel(body2, 0, 0, 0);
dBodySetAngularVel(body1, 0, 0, 0);
dBodySetAngularVel(body2, 0, 0, 0);
}
void start()
{
dMass mass;
world = dWorldCreate();
dWorldSetGravity (world,0,0,-9.8);
dWorldSetERP(world, 0.2);
space = dSimpleSpaceCreate (0);
body1 = dBodyCreate(world);
body2 = dBodyCreate(world);
dBodySetFiniteRotationMode(body1, 1);
dBodySetFiniteRotationMode(body2, 1);
geom1 = dCreateCylinder(space, 0.2, 0.5);
dGeomSetBody(geom1, body1);
dMassSetCylinder(&mass, 100, 3, 0.2, 0.5);
dBodySetMass(body1, &mass);
geom2 = dCreateCylinder(space, 0.2, 0.5);
dGeomSetBody(geom2, body2);
dMassSetCylinder(&mass, 100, 3, 0.2, 0.5);
dBodySetMass(body2, &mass);
hinge1 = dJointCreateHinge(world, 0);
dJointAttach(hinge1, body1, 0);
hinge2 = dJointCreateHinge(world, 0);
dJointAttach(hinge2, body2, 0);
transmission = dJointCreateTransmission(world, 0);
dJointAttach(transmission, body1, body2);
dJointSetFeedback(transmission, &feedback);
setup();
// initial camera position
static float xyz[3] = {1.15,-2.78,4.1};
static float hpr[3] = {105,-45.5,0};
dsSetViewpoint (xyz,hpr);
fprintf (stderr,
"The green wheel is driving the red one. To control it use the following:\n"
" '[' : decrease wheel ratio\n"
" ']' : increase wheel ratio\n"
" ',' : decrease driving wheel speed\n"
" '.' : increase driving wheel speed\n"
" '-' : decrease backlash\n"
" '=' : increase backlash\n"
" '1' : switch to parallel axes gears mode\n"
" '2' : switch to intersecting axes gears mode\n"
" '3' : switch to chain (or belt) mode\n"
);
}
void stop()
{
dSpaceDestroy(space);
dWorldDestroy(world);
}
void drawGeom(dGeomID g)
{
int gclass = dGeomGetClass(g);
const dReal *pos = dGeomGetPosition(g);
const dReal *rot = dGeomGetRotation(g);
switch (gclass) {
case dCylinderClass:
{
dReal length, radius;
if (g == geom1) {
dsSetColorAlpha(1, 0, 0, 1);
} else {
dsSetColorAlpha(0, 1, 0, 1);
}
dsSetTexture (DS_WOOD);
dGeomCylinderGetParams(g, &radius, &length);
dsDrawCylinder(pos, rot, length, radius);
break;
}
default:
{
abort();
}
}
}
void simLoop(int pause)
{
if (!pause) {
const dReal step = 0.003;
const unsigned nsteps = 4;
for (unsigned i=0; i<nsteps; ++i) {
dWorldQuickStep(world, step);
}
}
#if 0
{
const dReal *omega_1, *omega_2;
omega_1 = dBodyGetAngularVel(body1);
omega_2 = dBodyGetAngularVel(body2);
printf ("T1: %f, %f, %f\n",
feedback.t1[0], feedback.t1[1], feedback.t1[2]);
printf ("T2: %f, %f, %f\n",
feedback.t2[0], feedback.t2[1], feedback.t2[2]);
printf ("F1: %f, %f, %f\n",
feedback.f1[0], feedback.f1[1], feedback.f1[2]);
printf ("F2: %f, %f, %f\n",
feedback.f2[0], feedback.f2[1], feedback.f2[2]);
}
#endif
// now we draw everything
unsigned ngeoms = dSpaceGetNumGeoms(space);
for (unsigned i=0; i<ngeoms; ++i) {
dGeomID g = dSpaceGetGeom(space, i);
drawGeom(g);
}
const dReal *R_1 = dGeomGetRotation(geom1);
const dReal *R_2 = dGeomGetRotation(geom2);
dVector3 c_1, c_2, a_1, a_2;
dJointGetTransmissionContactPoint1(transmission, c_1);
dJointGetTransmissionContactPoint2(transmission, c_2);
dJointGetTransmissionAnchor1(transmission, a_1);
dJointGetTransmissionAnchor2(transmission, a_2);
dsSetColorAlpha(1, 0, 0, 0.5);
dsDrawCylinder(a_1, R_1, 0.05, dCalcPointsDistance3(c_1, a_1));
dsSetColorAlpha(0, 1, 0, 0.5);
dsDrawCylinder(a_2, R_2, 0.05, dCalcPointsDistance3(c_2, a_2));
dsSetColorAlpha(1, 0, 0, 0.5);
dsDrawSphere (c_1, R_1, 0.05);
dsDrawSphere (c_2, R_1, 0.05);
dsSetColorAlpha(1, 1, 0, 0.5);
if (mode == dTransmissionChainDrive) {
dsDrawLine(c_1, c_2);
}
}
static void command (int cmd)
{
if (cmd == '[') {
switch(mode) {
case dTransmissionParallelAxes:
if (ratio > 0.125) {
ratio *= 0.5;
fprintf (stderr, "Gear ratio set to %.3f.\n", ratio);
}
break;
case dTransmissionIntersectingAxes:
if (theta > 0.1) {
theta -= 0.1;
fprintf (stderr, "Gear angle set to %.3f deg.\n",
theta / M_PI * 180);
}
break;
case dTransmissionChainDrive:
if (rho_2 > 0.125) {
rho_2 /= 2;
fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1);
}
break;
}
setup();
} else if (cmd == ']') {
switch(mode) {
case dTransmissionParallelAxes:
if (ratio < 8) {
ratio *= 2;
fprintf (stderr, "Gear ratio set to %.3f.\n", ratio);
}
break;
case dTransmissionIntersectingAxes:
if (theta < 0.9) {
theta += 0.1;
fprintf (stderr, "Gear angle set to %.3f deg.\n",
theta / M_PI * 180);
}
break;
case dTransmissionChainDrive:
if (rho_2 < 2) {
rho_2 *= 2;
fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1);
}
break;
}
setup();
} else if (cmd == '.') {
speed += 5;
fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed);
dJointSetHingeParam(hinge2, dParamVel, speed);
} else if (cmd == ',') {
speed -= 5;
fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed);
dJointSetHingeParam(hinge2, dParamVel, speed);
} else if (cmd == '/') {
if (dJointGetHingeParam(hinge2, dParamFMax) > 0) {
dJointSetHingeParam(hinge2, dParamFMax, 0);
} else {
dJointSetHingeParam(hinge2, dParamFMax, 50);
}
} else if (cmd == '-') {
backlash -= 0.1;
fprintf (stderr, "Backlash set to %g m.\n", backlash);
dJointSetTransmissionBacklash(transmission, backlash);
} else if (cmd == '=') {
backlash += 0.1;
fprintf (stderr, "Backlash set to %g m.\n", backlash);
dJointSetTransmissionBacklash(transmission, backlash);
} else if (cmd == '1') {
mode = dTransmissionParallelAxes;
setup();
} else if (cmd == '2') {
mode = dTransmissionIntersectingAxes;
setup();
} else if (cmd == '3') {
mode = dTransmissionChainDrive;
setup();
}
}
int main(int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = stop;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE();
// run demo
dsSimulationLoop (argc, argv, 800, 600, &fn);
dCloseODE();
return 0;
}

605
thirdparty/ode-0.16.5/ode/demo/demo_trimesh.cpp vendored Normal file
View File

@@ -0,0 +1,605 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// TriMesh test by Erwin de Vries
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
//<---- Convex Object
static const dReal planes[] = // planes for a cube
{
1.0f ,0.0f ,0.0f ,0.25f,
0.0f ,1.0f ,0.0f ,0.25f,
0.0f ,0.0f ,1.0f ,0.25f,
0.0f ,0.0f ,-1.0f,0.25f,
0.0f ,-1.0f,0.0f ,0.25f,
-1.0f,0.0f ,0.0f ,0.25f
/*
1.0f ,0.0f ,0.0f ,2.0f,
0.0f ,1.0f ,0.0f ,1.0f,
0.0f ,0.0f ,1.0f ,1.0f,
0.0f ,0.0f ,-1.0f,1.0f,
0.0f ,-1.0f,0.0f ,1.0f,
-1.0f,0.0f ,0.0f ,0.0f
*/
};
static const unsigned int planecount=6;
static const dReal points[] = // points for a cube
{
0.25f,0.25f,0.25f,
-0.25f,0.25f,0.25f,
0.25f,-0.25f,0.25f,
-0.25f,-0.25f,0.25f,
0.25f,0.25f,-0.25f,
-0.25f,0.25f,-0.25f,
0.25f,-0.25f,-0.25f,
-0.25f,-0.25f,-0.25f,
};
static const unsigned int pointcount=8;
static const unsigned int polygons[] = //Polygons for a cube (6 squares)
{
4,0,2,6,4, // positive X
4,1,0,4,5, // positive Y
4,0,1,3,2, // positive Z
4,3,1,5,7, // negative X
4,2,3,7,6, // negative Y
4,5,4,6,7, // negative Z
};
//----> Convex Object
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawLine dsDrawLineD
#define dsDrawTriangle dsDrawTriangleD
#define dsDrawConvex dsDrawConvexD
#endif
// some constants
#define NUM 200 // max number of objects
#define DENSITY (5.0) // density of all objects
#define GPB 3 // maximum number of geometries per body
#define MAX_CONTACTS 40 // maximum number of contact points per body
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom[GPB]; // geometries representing this body
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected = -1; // selected object
static int show_aabb = 0; // show geom AABBs?
static int show_contacts = 0; // show contact points?
static int random_pos = 1; // drop objects from random position?
#define VertexCount 5
#define IndexCount 12
static dVector3 Size;
static float Vertices[VertexCount][3];
static dTriIndex Indices[IndexCount];
static dGeomID TriMesh;
static dGeomID Ray;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i<MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
for (i=0; i<numc; i++) {
if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){
dMatrix3 Rotation;
dRSetIdentity(Rotation);
dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01));
dVector3 End;
End[0] = contact[i].geom.pos[0] + (contact[i].geom.normal[0] * contact[i].geom.depth);
End[1] = contact[i].geom.pos[1] + (contact[i].geom.normal[1] * contact[i].geom.depth);
End[2] = contact[i].geom.pos[2] + (contact[i].geom.normal[2] * contact[i].geom.depth);
End[3] = contact[i].geom.pos[3] + (contact[i].geom.normal[3] * contact[i].geom.depth);
dsDrawLine(contact[i].geom.pos, End);
continue;
}
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
}
}
}
// start simulation - set viewpoint
static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);
static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("To drop another object, press:\n");
printf (" b for box.\n");
printf (" s for sphere.\n");
printf (" c for cylinder.\n");
printf( " v for a convex.\n" );
printf (" x for a composite object.\n");
printf ("To select an object, press space.\n");
printf ("To disable the selected object, press d.\n");
printf ("To enable the selected object, press e.\n");
printf ("To toggle showing the geom AABBs, press a.\n");
printf ("To toggle showing the contact points, press t.\n");
printf ("To toggle dropping from random position/orientation, press r.\n");
}
char locase (char c)
{
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(dsizeint)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
// draw a geom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
} else if (type == dConvexClass) {
//dVector3 sides={0.50,0.50,0.50};
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
}
/*
// cylinder option not yet implemented
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
*/
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
// simulation loop
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
//if (!pause) dWorldStepFast (world,0.05, 1);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
/*{
for (int i = 1; i < IndexCount; i++) {
dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
}
}*/
{const dReal* Pos = dGeomGetPosition(TriMesh);
const dReal* Rot = dGeomGetRotation(TriMesh);
{for (int i = 0; i < IndexCount / 3; i++){
const float *p = Vertices[Indices[i * 3 + 0]];
const dVector3 v0 = { p[0], p[1], p[2] };
p = Vertices[Indices[i * 3 + 1]];
const dVector3 v1 = { p[0], p[1], p[2] };
p = Vertices[Indices[i * 3 + 2]];
const dVector3 v2 = { p[0], p[1], p[2] };
dsDrawTriangle(Pos, Rot, v0, v1, v2, 0);
}}}
if (Ray){
dVector3 Origin, Direction;
dGeomRayGet(Ray, Origin, Direction);
dReal Length = dGeomRayGetLength(Ray);
dVector3 End;
End[0] = Origin[0] + (Direction[0] * Length);
End[1] = Origin[1] + (Direction[1] * Length);
End[2] = Origin[2] + (Direction[2] * Length);
End[3] = Origin[3] + (Direction[3] * Length);
dsDrawLine(Origin, End);
}
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dSimpleSpaceCreate(0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
//dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
Size[0] = 5.0f;
Size[1] = 5.0f;
Size[2] = 2.5f;
Vertices[0][0] = -Size[0];
Vertices[0][1] = -Size[1];
Vertices[0][2] = Size[2];
Vertices[1][0] = Size[0];
Vertices[1][1] = -Size[1];
Vertices[1][2] = Size[2];
Vertices[2][0] = Size[0];
Vertices[2][1] = Size[1];
Vertices[2][2] = Size[2];
Vertices[3][0] = -Size[0];
Vertices[3][1] = Size[1];
Vertices[3][2] = Size[2];
Vertices[4][0] = 0;
Vertices[4][1] = 0;
Vertices[4][2] = 0;
Indices[0] = 0;
Indices[1] = 1;
Indices[2] = 4;
Indices[3] = 1;
Indices[4] = 2;
Indices[5] = 4;
Indices[6] = 2;
Indices[7] = 3;
Indices[8] = 4;
Indices[9] = 3;
Indices[10] = 0;
Indices[11] = 4;
dTriMeshDataID Data = dGeomTriMeshDataCreate();
//dGeomTriMeshDataBuildSimple(Data, (dReal*)Vertices, VertexCount, Indices, IndexCount);
dGeomTriMeshDataBuildSingle(Data, Vertices[0], 3 * sizeof(float), VertexCount, &Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(Data, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);
TriMesh = dCreateTriMesh(space, Data, 0, 0, 0);
//dGeomSetPosition(TriMesh, 0, 0, 1.0);
Ray = dCreateRay(space, 0.9);
dVector3 Origin, Direction;
Origin[0] = 0.0;
Origin[1] = 0;
Origin[2] = 0.5;
Origin[3] = 0;
Direction[0] = 0;
Direction[1] = 1.1f;
Direction[2] = -1;
Direction[3] = 0;
dNormalize3(Direction);
dGeomRaySet(Ray, Origin[0], Origin[1], Origin[2], Direction[0], Direction[1], Direction[2]);
dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
// dWorldSetStepIslandsProcessingMaxThreadCount(world, 1);
dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dThreadingImplementationShutdownProcessing(threading);
dThreadingFreeThreadPool(pool);
dWorldSetStepThreadingImplementation(world, NULL, NULL);
dThreadingFreeImplementation(threading);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}

2271
thirdparty/ode-0.16.5/ode/demo/halton235_geom.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

216
thirdparty/ode-0.16.5/ode/demo/icosahedron_geom.h vendored Normal file
View File

@@ -0,0 +1,216 @@
//<---- Icosahedron ---->
/*
This is a description of a convex icosahedron, to test
the convex collision detection.
*/
unsigned int Sphere_pointcount = 42;
unsigned int Sphere_planecount = 80;
dReal Sphere_points[126]={
0.000000,0.000000,-0.300000,
0.217080,-0.157716,-0.134164,
-0.082915,-0.255192,-0.134164,
-0.268327,0.000000,-0.134164,
-0.082915,0.255192,-0.134164,
0.217080,0.157716,-0.134164,
0.082915,-0.255192,0.134164,
-0.217080,-0.157716,0.134164,
-0.217080,0.157716,0.134164,
0.082915,0.255192,0.134164,
0.268327,0.000000,0.134164,
0.000000,0.000000,0.300000,
0.127597,-0.092703,-0.255196,
-0.048737,-0.149999,-0.255196,
0.078861,-0.242703,-0.157721,
0.127597,0.092703,-0.255196,
0.255194,0.000000,-0.157721,
-0.157719,0.000000,-0.255195,
-0.206457,-0.149999,-0.157721,
-0.048737,0.149999,-0.255196,
-0.206457,0.149999,-0.157721,
0.078861,0.242703,-0.157721,
0.285317,0.092704,0.000000,
0.285317,-0.092704,0.000000,
0.176336,-0.242705,0.000000,
0.000000,-0.300000,0.000000,
-0.176336,-0.242705,0.000000,
-0.285317,-0.092704,0.000000,
-0.285317,0.092704,0.000000,
-0.176336,0.242705,0.000000,
0.000000,0.300000,0.000000,
0.176336,0.242705,0.000000,
0.206457,-0.149999,0.157721,
-0.078861,-0.242703,0.157721,
-0.255194,0.000000,0.157721,
-0.078861,0.242703,0.157721,
0.206457,0.149999,0.157721,
0.157719,0.000000,0.255195,
0.048737,-0.149999,0.255196,
-0.127597,-0.092703,0.255196,
-0.127597,0.092703,0.255196,
0.048737,0.149999,0.255196
};
unsigned int Sphere_polygons[]={
3,14,12,1,
3,12,14,13,
3,2,13,14,
3,13,0,12,
3,16,1,12,
3,12,15,16,
3,5,16,15,
3,12,0,15,
3,18,13,2,
3,13,18,17,
3,3,17,18,
3,17,0,13,
3,20,17,3,
3,17,20,19,
3,4,19,20,
3,19,0,17,
3,21,19,4,
3,19,21,15,
3,5,15,21,
3,15,0,19,
3,23,1,16,
3,16,22,23,
3,10,23,22,
3,22,16,5,
3,25,2,14,
3,14,24,25,
3,6,25,24,
3,24,14,1,
3,27,3,18,
3,18,26,27,
3,7,27,26,
3,26,18,2,
3,29,4,20,
3,20,28,29,
3,8,29,28,
3,28,20,3,
3,31,5,21,
3,21,30,31,
3,9,31,30,
3,30,21,4,
3,32,23,10,
3,23,32,24,
3,6,24,32,
3,24,1,23,
3,33,25,6,
3,25,33,26,
3,7,26,33,
3,26,2,25,
3,34,27,7,
3,27,34,28,
3,8,28,34,
3,28,3,27,
3,35,29,8,
3,29,35,30,
3,9,30,35,
3,30,4,29,
3,36,31,9,
3,31,36,22,
3,10,22,36,
3,22,5,31,
3,38,6,32,
3,32,37,38,
3,11,38,37,
3,37,32,10,
3,39,7,33,
3,33,38,39,
3,11,39,38,
3,38,33,6,
3,40,8,34,
3,34,39,40,
3,11,40,39,
3,39,34,7,
3,41,9,35,
3,35,40,41,
3,11,41,40,
3,40,35,8,
3,37,10,36,
3,36,41,37,
3,11,37,41,
3,41,36,9,
};
dReal Sphere_planes[]={
0.471317,-0.583121,-0.661687,0.283056,
0.187594,-0.577345,-0.794658,0.280252,
-0.038547,-0.748789,-0.661687,0.283056,
0.102381,-0.315090,-0.943523,0.283057,
0.700228,-0.268049,-0.661688,0.283056,
0.607060,0.000000,-0.794656,0.280252,
0.700228,0.268049,-0.661688,0.283056,
0.331305,0.000000,-0.943524,0.283057,
-0.408939,-0.628443,-0.661686,0.283056,
-0.491119,-0.356821,-0.794657,0.280252,
-0.724044,-0.194735,-0.661694,0.283057,
-0.268034,-0.194737,-0.943523,0.283057,
-0.724044,0.194735,-0.661694,0.283057,
-0.491119,0.356821,-0.794657,0.280252,
-0.408939,0.628443,-0.661686,0.283056,
-0.268034,0.194737,-0.943523,0.283057,
-0.038547,0.748789,-0.661687,0.283056,
0.187594,0.577345,-0.794658,0.280252,
0.471317,0.583121,-0.661687,0.283056,
0.102381,0.315090,-0.943523,0.283057,
0.904981,-0.268049,-0.330393,0.283056,
0.982246,0.000000,-0.187599,0.280252,
0.992077,0.000000,0.125631,0.283057,
0.904981,0.268049,-0.330393,0.283056,
0.024726,-0.943519,-0.330396,0.283056,
0.303531,-0.934171,-0.187598,0.280251,
0.306568,-0.943519,0.125651,0.283056,
0.534590,-0.777851,-0.330395,0.283056,
-0.889698,-0.315092,-0.330386,0.283056,
-0.794656,-0.577348,-0.187595,0.280251,
-0.802607,-0.583125,0.125648,0.283055,
-0.574584,-0.748793,-0.330397,0.283055,
-0.574584,0.748793,-0.330397,0.283055,
-0.794656,0.577348,-0.187595,0.280251,
-0.802607,0.583125,0.125648,0.283055,
-0.889698,0.315092,-0.330386,0.283056,
0.534590,0.777851,-0.330395,0.283056,
0.303531,0.934171,-0.187598,0.280251,
0.306568,0.943519,0.125651,0.283056,
0.024726,0.943519,-0.330396,0.283056,
0.889698,-0.315092,0.330386,0.283056,
0.794656,-0.577348,0.187595,0.280251,
0.574584,-0.748793,0.330397,0.283055,
0.802607,-0.583125,-0.125648,0.283055,
-0.024726,-0.943519,0.330396,0.283055,
-0.303531,-0.934171,0.187598,0.280251,
-0.534590,-0.777851,0.330395,0.283056,
-0.306568,-0.943519,-0.125651,0.283056,
-0.904981,-0.268049,0.330393,0.283056,
-0.982246,0.000000,0.187599,0.280252,
-0.904981,0.268049,0.330393,0.283056,
-0.992077,0.000000,-0.125631,0.283057,
-0.534590,0.777851,0.330395,0.283056,
-0.303531,0.934171,0.187598,0.280251,
-0.024726,0.943519,0.330396,0.283055,
-0.306568,0.943519,-0.125651,0.283056,
0.574584,0.748793,0.330397,0.283055,
0.794656,0.577348,0.187595,0.280251,
0.889698,0.315092,0.330386,0.283056,
0.802607,0.583125,-0.125648,0.283055,
0.408939,-0.628443,0.661686,0.283056,
0.491119,-0.356821,0.794657,0.280252,
0.268034,-0.194737,0.943523,0.283057,
0.724044,-0.194735,0.661694,0.283057,
-0.471317,-0.583121,0.661687,0.283056,
-0.187594,-0.577345,0.794658,0.280252,
-0.102381,-0.315090,0.943523,0.283057,
0.038547,-0.748789,0.661687,0.283056,
-0.700228,0.268049,0.661688,0.283056,
-0.607060,0.000000,0.794656,0.280252,
-0.331305,0.000000,0.943524,0.283057,
-0.700228,-0.268049,0.661688,0.283056,
0.038547,0.748789,0.661687,0.283056,
-0.187594,0.577345,0.794658,0.280252,
-0.102381,0.315090,0.943523,0.283057,
-0.471317,0.583121,0.661687,0.283056,
0.724044,0.194735,0.661694,0.283057,
0.491119,0.356821,0.794657,0.280252,
0.268034,0.194737,0.943523,0.283057,
0.408939,0.628443,0.661686,0.283056,
};

26
thirdparty/ode-0.16.5/ode/demo/texturepath.h vendored Normal file
View File

@@ -0,0 +1,26 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#ifndef DRAWSTUFF_TEXTURE_PATH
#define DRAWSTUFF_TEXTURE_PATH "../../drawstuff/textures"
#endif

9
thirdparty/ode-0.16.5/ode/demo/world_geom3.h vendored Normal file
View File

File diff suppressed because one or more lines are too long