rev: 3827f53bae88e48a594610914ba74b9512ead6dd tukan/testing/test_ode.sc -rw-r--r-- 4.6 KiB View raw Log this file
3827f53bae88 — Leonard Ritter * more work on module system 3 months ago
                                                                                
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
using import glm
using import Capture
using import Array
using import enum

import ..tukan.use
using import tukan.VertexPainter
using import tukan.GLMain
using import tukan.gl
using import tukan.ode
using import tukan.rotation
using import tukan.math

global world = (World)
world.gravity = (vec3 0 -10 0)

global space = (SimpleSpace)

local geoms : (Array Geom)
local bodies : (Array Body)
local joints : (Array PUJoint)
local amotorjoints : (Array AMotorJoint)

do
    let planeg =
        Plane space (vec4 0 1 0 0)
    let planeb =
        Body world
    planeg.body = planeb
    planeb.kinematic = true

    inline makebox (y r)
        #let g = (Cylinder space r (2.0 * r))
        let g = (Capsule space (r * 0.5) (2.0 * r - r * 1.0))
        #let g = (Box space (2.0 * r) (2.0 * r) (2.0 * r))
        #let g = (Sphere space r)
        let b = (Body world)
        b.position = (vec3 0 y 0)
        b.quaternion = (versor (vec3 1 0 0) (pi * 0.5))
        g.body = b
        _ g b
    inline append (g b)
        'append geoms g
        'append bodies b

    let y = 0.51
    let r = 0.5
    let box1g box1b = (makebox y r)
    let j0 = (PUJoint world)
    'attach j0 planeb box1b
    j0.anchor = (vec3 0)
    j0.axis1 = (vec3 1 0 0)
    j0.axis2 = (vec3 0 0 1)
    j0.axis3 = (vec3 0 1 0)
    'set-lo-stop j0 2 0.0
    'set-hi-stop j0 2 0.0
    'append joints j0
    append planeg planeb

    #D := 0.1
    D := 0.0
    #D := -0.01

    let boxg boxb =
        fold (boxg boxb y r = box1g box1b y r) for i in (range 9)
            y := y + r + D
            r := r * 0.9
            y := y + r
            let box2g box2b = (makebox y r)
            do
                let j = (PUJoint world)
                'attach j boxb box2b
                j.anchor = (boxb.position as vec3 + box2b.position as vec3) * 0.5
                j.axis1 = (vec3 1 0 0)
                j.axis2 = (vec3 0 0 1)
                j.axis3 = (vec3 0 1 0)
                'set-lo-stop j 2 0.0
                'set-hi-stop j 2 0.0
                'append joints j

            #do
                let j = (AMotorJoint world)
                'attach j boxb box2b
                'set-num-axes j 2
                'set-axis j 0 1 (vec3 1 0 0)
                'set-axis j 1 1 (vec3 0 1 0)
                'append amotorjoints j
            append boxg boxb
            _ box2g box2b y r
    append boxg boxb

global jgroup = (JointGroup)

let glmain =
    GLMain
        title = "ODE Test"
        resizable = true
        width = 1280
        height = 720

global t = 0.0
let NUM_CONTACTS = 1024

@@ 'on GLMain.on-draw
inline (time size glmain)
    let N = 2
    a := t * pi / 8.0
    g := (normalize (vec3 (cos a) -2 (sin a))) * 10.0
    world.gravity = g
    for i in (range N)
        'step world (/ 1.0 (N * 60.0))
        'empty jgroup
        'collide space
            fn (o1 o2)
                let contacts = (alloca-array Contact NUM_CONTACTS)
                for i in (range NUM_CONTACTS)
                    contacts @ i = (Contact)
                let num_collisions =
                    'collide o1 o2 NUM_CONTACTS
                        & (contacts @ 0 . geom)
                        sizeof Contact
                for j in (range 0 num_collisions)
                    let surf = (contacts @ j . surface)
                    surf.mode = (ContactBounce as u32 as i32)
                    surf.mu = 250
                    surf.bounce = 0.6
                    surf.bounce_vel = 0.05
                    let c =
                        ContactJoint world jgroup (contacts @ j)
                    let b0 =
                        contacts @ j . geom . g1 . body
                    let b1 =
                        contacts @ j . geom . g2 . body
                    'attach c b0 b1
        #for j m in (zip joints amotorjoints)
            print
                j.angle1 as f32; j.angle2 as f32
                \ ('get-angle m 0) ('get-angle m 1)

    #fn draw (time size glmain)
    let painter = (VertexPainter)
    'setup-depth painter
    GL.BindFramebuffer GL.FRAMEBUFFER 0
    GL.Viewport 0 0 (i32 size.x) (i32 size.y)
    GL.ClearColor 0 0 0.5 1
    GL.Enable GL.CULL_FACE
    GL.Clear
        |
            GL.COLOR_BUFFER_BIT
            GL.DEPTH_BUFFER_BIT
            GL.STENCIL_BUFFER_BIT

    t += (/ 60.0)

    from (methodsof painter) let clear color screen rotate AABB \
        translate quad perspective scale light flush wireframe moveto lineto \
        closepath fill stroke push pop
    # setup view
    light 0.3 -1 -0.2
    scale 2.0
    perspective size
    translate
        vec3 0 -4 10
    rotate
        radians 45.0
        #pi * t * 0.25
        vec3 0 1 0
    'debugdraw space painter
    drop painter

'run glmain

;