forked from ianremmler/ode
/
geom.go
536 lines (454 loc) · 14.9 KB
/
geom.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
package ode
// #include <ode/ode.h>
// extern void callNearCallback(void *data, dGeomID obj1, dGeomID obj2);
import "C"
import (
"unsafe"
)
// Geometry classes
const (
SphereClass = C.dSphereClass
BoxClass = C.dBoxClass
CapsuleClass = C.dCapsuleClass
CylinderClass = C.dCylinderClass
PlaneClass = C.dPlaneClass
RayClass = C.dRayClass
ConvexClass = C.dConvexClass
TriMeshClass = C.dTriMeshClass
HeightfieldClass = C.dHeightfieldClass
SimpleSpaceClass = C.dSimpleSpaceClass
HashSpaceClass = C.dHashSpaceClass
SweepAndPruneSpaceClass = C.dSweepAndPruneSpaceClass
QuadTreeSpaceClass = C.dQuadTreeSpaceClass
NumClasses = C.dGeomNumClasses
MaxUserClasses = C.dMaxUserClasses
FirstUserClass = C.dFirstUserClass
LastUserClass = C.dLastUserClass
FirstSpaceClass = C.dFirstSpaceClass
LastSpaceClass = C.dLastSpaceClass
)
var (
geomData = map[Geom]interface{}{}
)
// Geom represents rigid body geometry.
type Geom interface {
c() C.dGeomID
Destroy()
SetData(data interface{})
Data() interface{}
SetBody(body Body)
Body() Body
SetPosition(pos Vector3)
Position() Vector3
SetRotation(rot Matrix3)
Rotation() Matrix3
SetQuaternion(quat Quaternion)
Quaternion() Quaternion
AABB() AABB
IsSpace() bool
Space() Space
Class() int
SetCategoryBits(bits int)
SetCollideBits(bits int)
CategoryBits() int
CollideBits() int
SetEnabled(isEnabled bool)
Enabled() bool
RelPointPos(pt Vector3) Vector3
PosRelPoint(pos Vector3) Vector3
VectorToWorld(vec Vector3) Vector3
VectorFromWorld(wld Vector3) Vector3
OffsetPosition() Vector3
SetOffsetPosition(pos Vector3)
OffsetRotation() Matrix3
SetOffsetRotation(rot Matrix3)
OffsetQuaternion() Quaternion
SetOffsetQuaternion(quat Quaternion)
SetOffsetWorldPosition(pos Vector3)
SetOffsetWorldRotation(rot Matrix3)
SetOffsetWorldQuaternion(quat Quaternion)
ClearOffset()
IsOffset() bool
Collide(other Geom, maxContacts uint16, flags int) []ContactGeom
Collide2(other Geom, data interface{}, cb NearCallback)
Next() Geom
}
// GeomBase implements Geom, and is embedded by specific Geom types.
type GeomBase uintptr
func cToGeom(c C.dGeomID) Geom {
base := GeomBase(unsafe.Pointer(c))
var g Geom
switch int(C.dGeomGetClass(c)) {
case SphereClass:
g = Sphere{base}
case BoxClass:
g = Box{base}
case CapsuleClass:
g = Capsule{base}
case CylinderClass:
g = Cylinder{base}
case PlaneClass:
g = Plane{base}
case RayClass:
g = Ray{base}
case HeightfieldClass:
g = Heightfield{base}
case TriMeshClass:
g = TriMesh{base}
default:
g = base
}
return g
}
func (g GeomBase) c() C.dGeomID {
return C.dGeomID(unsafe.Pointer(g))
}
// Destroy destroys the GeomBase.
func (g GeomBase) Destroy() {
delete(geomData, g)
C.dGeomDestroy(g.c())
}
// SetData associates user-specified data with the geometry.
func (g GeomBase) SetData(data interface{}) {
geomData[g] = data
}
// Data returns the user-specified data associated with the geometry.
func (g GeomBase) Data() interface{} {
return geomData[g]
}
// SetBody sets the associated body.
func (g GeomBase) SetBody(body Body) {
C.dGeomSetBody(g.c(), body.c())
}
// Body returns the body associated with the geometry.
func (g GeomBase) Body() Body {
return cToBody(C.dGeomGetBody(g.c()))
}
// SetPosition sets the position.
func (g GeomBase) SetPosition(pos Vector3) {
C.dGeomSetPosition(g.c(), C.dReal(pos[0]), C.dReal(pos[1]), C.dReal(pos[2]))
}
// Position returns the position.
func (g GeomBase) Position() Vector3 {
pos := NewVector3()
C.dGeomCopyPosition(g.c(), (*C.dReal)(&pos[0]))
return pos
}
// SetRotation sets the orientation represented by a rotation matrix.
func (g GeomBase) SetRotation(rot Matrix3) {
C.dGeomSetRotation(g.c(), (*C.dReal)(&rot[0][0]))
}
// Rotation returns the orientation represented by a rotation matrix.
func (g GeomBase) Rotation() Matrix3 {
rot := NewMatrix3()
C.dGeomCopyRotation(g.c(), (*C.dReal)(&rot[0][0]))
return rot
}
// SetQuaternion sets the orientation represented by a quaternion.
func (g GeomBase) SetQuaternion(quat Quaternion) {
C.dGeomSetQuaternion(g.c(), (*C.dReal)(&quat[0]))
}
// Quaternion returns the orientation represented by a quaternion.
func (g GeomBase) Quaternion() Quaternion {
quat := NewQuaternion()
C.dGeomGetQuaternion(g.c(), (*C.dReal)(&quat[0]))
return quat
}
// AABB returns the axis-aligned bounding box.
func (g GeomBase) AABB() AABB {
aabb := NewAABB()
C.dGeomGetAABB(g.c(), (*C.dReal)(&aabb[0]))
return aabb
}
// IsSpace returns whether the geometry is a space.
func (g GeomBase) IsSpace() bool {
return C.dGeomIsSpace(g.c()) != 0
}
// Space returns the containing space.
func (g GeomBase) Space() Space {
return cToSpace(C.dGeomGetSpace(g.c()))
}
// Class returns the geometry class.
func (g GeomBase) Class() int {
return int(C.dGeomGetClass(g.c()))
}
// SetCategoryBits sets the category bitfield.
func (g GeomBase) SetCategoryBits(bits int) {
C.dGeomSetCategoryBits(g.c(), C.ulong(bits))
}
// CategoryBits returns the category bitfield.
func (g GeomBase) CategoryBits() int {
return int(C.dGeomGetCategoryBits(g.c()))
}
// SetCollideBits sets the collide bitfield.
func (g GeomBase) SetCollideBits(bits int) {
C.dGeomSetCollideBits(g.c(), C.ulong(bits))
}
// CollideBits returns the collide bitfield.
func (g GeomBase) CollideBits() int {
return int(C.dGeomGetCollideBits(g.c()))
}
// SetEnabled sets whether the geometry is enabled.
func (g GeomBase) SetEnabled(isEnabled bool) {
if isEnabled {
C.dGeomEnable(g.c())
} else {
C.dGeomDisable(g.c())
}
}
// Enabled returns whether the geometry is enabled.
func (g GeomBase) Enabled() bool {
return bool(C.dGeomIsEnabled(g.c()) != 0)
}
// RelPointPos returns the position in world coordinates of a point in geometry
// coordinates.
func (g GeomBase) RelPointPos(pt Vector3) Vector3 {
pos := NewVector3()
C.dGeomGetRelPointPos(g.c(), C.dReal(pt[0]), C.dReal(pt[1]), C.dReal(pt[2]), (*C.dReal)(&pos[0]))
return pos
}
// PosRelPoint returns the position in geometry coordinates of a point in world
// coordinates.
func (g GeomBase) PosRelPoint(pos Vector3) Vector3 {
pt := NewVector3()
C.dGeomGetPosRelPoint(g.c(), C.dReal(pos[0]), C.dReal(pos[1]), C.dReal(pos[2]), (*C.dReal)(&pt[0]))
return pt
}
// VectorToWorld converts a vector in geometry coordinates to world
// coordinates.
func (g GeomBase) VectorToWorld(vec Vector3) Vector3 {
wld := NewVector3()
C.dGeomVectorToWorld(g.c(), C.dReal(vec[0]), C.dReal(vec[1]), C.dReal(vec[2]), (*C.dReal)(&wld[0]))
return wld
}
// VectorFromWorld converts a vector in world coordinates to geometry
// coordinates.
func (g GeomBase) VectorFromWorld(wld Vector3) Vector3 {
vec := NewVector3()
C.dGeomVectorFromWorld(g.c(), C.dReal(wld[0]), C.dReal(wld[1]), C.dReal(wld[2]), (*C.dReal)(&vec[0]))
return vec
}
// SetOffsetPosition sets the position offset from the body.
func (g GeomBase) SetOffsetPosition(pos Vector3) {
C.dGeomSetOffsetPosition(g.c(), C.dReal(pos[0]), C.dReal(pos[1]), C.dReal(pos[2]))
}
// OffsetPosition returns the position offset from the body.
func (g GeomBase) OffsetPosition() Vector3 {
pos := NewVector3()
C.dGeomCopyOffsetPosition(g.c(), (*C.dReal)(&pos[0]))
return pos
}
// SetOffsetRotation sets the orientation offset from the body represented by a
// rotation matrix.
func (g GeomBase) SetOffsetRotation(rot Matrix3) {
C.dGeomSetOffsetRotation(g.c(), (*C.dReal)(&rot[0][0]))
}
// OffsetRotation returns the orientation offset from the body represented by a
// rotation matrix.
func (g GeomBase) OffsetRotation() Matrix3 {
rot := NewMatrix3()
C.dGeomCopyOffsetRotation(g.c(), (*C.dReal)(&rot[0][0]))
return rot
}
// SetOffsetQuaternion sets the offset from the body orientation represented by
// a quaternion.
func (g GeomBase) SetOffsetQuaternion(quat Quaternion) {
C.dGeomSetOffsetQuaternion(g.c(), (*C.dReal)(&quat[0]))
}
// OffsetQuaternion returns the orientation offset from the body represented by
// a quaternion.
func (g GeomBase) OffsetQuaternion() Quaternion {
quat := NewQuaternion()
C.dGeomGetOffsetQuaternion(g.c(), (*C.dReal)(&quat[0]))
return quat
}
// SetOffsetWorldPosition sets the offset to the body position such that the
// geom's world position is pos.
func (g GeomBase) SetOffsetWorldPosition(pos Vector3) {
C.dGeomSetOffsetWorldPosition(g.c(), C.dReal(pos[0]), C.dReal(pos[1]), C.dReal(pos[2]))
}
// SetOffsetWorldRotation sets the offset to the body orientation such that the
// geom's world orientation is represented by the matrix rot.
func (g GeomBase) SetOffsetWorldRotation(rot Matrix3) {
C.dGeomSetOffsetWorldRotation(g.c(), (*C.dReal)(&rot[0][0]))
}
// SetOffsetWorldQuaternion sets the offset to the body orientation such that
// the geom's world orientation is represented by the quaternion quat.
func (g GeomBase) SetOffsetWorldQuaternion(quat Quaternion) {
C.dGeomSetOffsetWorldQuaternion(g.c(), (*C.dReal)(&quat[0]))
}
// ClearOffset removes the body offset.
func (g GeomBase) ClearOffset() {
C.dGeomClearOffset(g.c())
}
// IsOffset returns whether a body offset has been created.
func (g GeomBase) IsOffset() bool {
return C.dGeomIsOffset(g.c()) != 0
}
// Collide tests for collision with the given geometry and returns a list of
// contact points.
func (g GeomBase) Collide(other Geom, maxContacts uint16, flags int) []ContactGeom {
cts := make([]C.dContactGeom, maxContacts)
numCts := int(C.dCollide(g.c(), other.c(), C.int(int(maxContacts)|flags), &cts[0],
C.int(unsafe.Sizeof(cts[0]))))
contacts := make([]ContactGeom, numCts)
for i := range contacts {
contacts[i] = *NewContactGeom()
contacts[i].fromC(&cts[i])
}
return contacts
}
// Collide2 tests for collision with the given geometry, applying cb for each
// contact.
func (g GeomBase) Collide2(other Geom, data interface{}, cb NearCallback) {
cbData := &nearCallbackData{fn: cb, data: data}
C.dSpaceCollide2(g.c(), other.c(), unsafe.Pointer(cbData),
(*C.dNearCallback)(C.callNearCallback))
}
// Next returns the next geometry.
func (g GeomBase) Next() Geom {
return cToGeom(C.dBodyGetNextGeom(g.c()))
}
// Sphere is a geometry representing a sphere.
type Sphere struct {
GeomBase
}
// SetRadius sets the radius.
func (s Sphere) SetRadius(radius float64) {
C.dGeomSphereSetRadius(s.c(), C.dReal(radius))
}
// Radius returns the radius.
func (s Sphere) Radius() float64 {
return float64(C.dGeomSphereGetRadius(s.c()))
}
// SpherePointDepth returns the depth of the given point.
func (s Sphere) SpherePointDepth(pt Vector3) float64 {
return float64(C.dGeomSpherePointDepth(s.c(), C.dReal(pt[0]), C.dReal(pt[1]), C.dReal(pt[2])))
}
// Box is a geometry representing a rectangular box.
type Box struct {
GeomBase
}
// SetLengths sets the lengths of the sides.
func (b Box) SetLengths(lens Vector3) {
C.dGeomBoxSetLengths(b.c(), C.dReal(lens[0]), C.dReal(lens[1]), C.dReal(lens[2]))
}
// Lengths returns the lengths of the sides.
func (b Box) Lengths() Vector3 {
lens := NewVector3()
C.dGeomBoxGetLengths(b.c(), (*C.dReal)(&lens[0]))
return lens
}
// PointDepth returns the depth of the given point.
func (b Box) PointDepth(pt Vector3) float64 {
return float64(C.dGeomBoxPointDepth(b.c(), C.dReal(pt[0]), C.dReal(pt[1]), C.dReal(pt[2])))
}
// Plane is a geometry that represents a plane.
type Plane struct {
GeomBase
}
// SetParams sets plane parameters.
func (p Plane) SetParams(params Vector4) {
C.dGeomPlaneSetParams(p.c(), C.dReal(params[0]), C.dReal(params[1]), C.dReal(params[2]), C.dReal(params[3]))
}
// Params returns plane parameters.
func (p Plane) Params() Vector4 {
params := NewVector4()
C.dGeomPlaneGetParams(p.c(), (*C.dReal)(¶ms[0]))
return params
}
// PointDepth returns the depth of the given point.
func (p Plane) PointDepth(pt Vector3) float64 {
return float64(C.dGeomPlanePointDepth(p.c(), C.dReal(pt[0]), C.dReal(pt[1]), C.dReal(pt[2])))
}
// Capsule is a geometry that represents a capsule (a cylinder with a half
// sphere on each end).
type Capsule struct {
GeomBase
}
// SetParams sets the radius and length.
func (c Capsule) SetParams(radius, length float64) {
C.dGeomCapsuleSetParams(c.c(), C.dReal(radius), C.dReal(length))
}
// Params returns the radius and length.
func (c Capsule) Params() (float64, float64) {
var radius, length float64
C.dGeomCapsuleGetParams(c.c(), (*C.dReal)(&radius), (*C.dReal)(&length))
return radius, length
}
// PointDepth returns the depth of the given point.
func (c Capsule) PointDepth(pt Vector3) float64 {
return float64(C.dGeomCapsulePointDepth(c.c(), C.dReal(pt[0]), C.dReal(pt[1]), C.dReal(pt[2])))
}
// Cylinder is a geometry that represents a cylider.
type Cylinder struct {
GeomBase
}
// SetParams sets the radius and length.
func (c Cylinder) SetParams(radius, length float64) {
C.dGeomCylinderSetParams(c.c(), C.dReal(radius), C.dReal(length))
}
// Params returns the radius and length.
func (c Cylinder) Params() (float64, float64) {
var radius, length float64
C.dGeomCylinderGetParams(c.c(), (*C.dReal)(&radius), (*C.dReal)(&length))
return radius, length
}
// Ray is a geometry representing a ray.
type Ray struct {
GeomBase
}
// SetPosDir sets the position and direction.
func (r Ray) SetPosDir(pos, dir Vector3) {
C.dGeomRaySet(r.c(), C.dReal(pos[0]), C.dReal(pos[1]), C.dReal(pos[2]),
C.dReal(dir[0]), C.dReal(dir[1]), C.dReal(dir[2]))
}
// PosDir returns the position and direction.
func (r Ray) PosDir() (Vector3, Vector3) {
pos, dir := NewVector3(), NewVector3()
C.dGeomRayGet(r.c(), (*C.dReal)(&pos[0]), (*C.dReal)(&dir[0]))
return pos, dir
}
// SetLength sets the length.
func (r Ray) SetLength(length float64) {
C.dGeomRaySetLength(r.c(), C.dReal(length))
}
// Length returns the length.
func (r Ray) Length() float64 {
return float64(C.dGeomRayGetLength(r.c()))
}
// SetFirstContact sets whether to stop collision detection after finding the
// first contact point.
func (r Ray) SetFirstContact(firstContact bool) {
C.dGeomRaySetFirstContact(r.c(), C.int(btoi(firstContact)))
}
// FirstContact returns whether collision detection will stop after finding the
// first contact.
func (r Ray) FirstContact() bool {
return C.dGeomRayGetFirstContact(r.c()) != 0
}
// SetBackfaceCull sets whether backface culling is enabled.
func (r Ray) SetBackfaceCull(backfaceCull bool) {
C.dGeomRaySetBackfaceCull(r.c(), C.int(btoi(backfaceCull)))
}
// BackfaceCull returns whether backface culling is enabled.
func (r Ray) BackfaceCull() bool {
return C.dGeomRayGetBackfaceCull(r.c()) != 0
}
// SetClosestHit sets whether to only report the closest hit.
func (r Ray) SetClosestHit(closestHit bool) {
C.dGeomRaySetClosestHit(r.c(), C.int(btoi(closestHit)))
}
// ClosestHit returns whether only the closest hit will be reported.
func (r Ray) ClosestHit() bool {
return C.dGeomRayGetClosestHit(r.c()) != 0
}
// Convex is a geometry representing a convex object.
type Convex struct {
GeomBase
}
// Set sets convex object data
func (c Convex) Set(planes PlaneList, pts VertexList, polyList PolygonList) {
C.dGeomSetConvex(c.c(), (*C.dReal)(&planes[0][0]), C.uint(len(planes)),
(*C.dReal)(&pts[0][0]), C.uint(len(pts)), &polyList[0])
}