Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs @ 216

Last change on this file since 216 was 216, checked in by mathiask, 17 years ago

[Physik] add ode-0.9
File size: 7.1 KB
Line 
1using System;
2using System.Collections.Generic;
3using Drawstuff.NET;
4
5namespace Ode.NET
6{
7#if dDOUBLE
8        using dReal = System.Double;
9#else
10        using dReal = System.Single;
11#endif
12
13        public class TestBoxStack
14        {
15                #region Description of convex shape
16
17                static dReal[] planes = 
18                        {
19                                1.0f, 0.0f, 0.0f, 0.25f,
20                                0.0f, 1.0f, 0.0f, 0.25f,
21                                0.0f, 0.0f, 1.0f, 0.25f,
22                                0.0f, 0.0f, -1.0f, 0.25f,
23                                0.0f, -1.0f, 0.0f, 0.25f,
24                                -1.0f, 0.0f , 0.0f, 0.25f
25                        };
26
27                static dReal[] points =
28                        {
29                                0.25f, 0.25f, 0.25f,
30                                -0.25f, 0.25f, 0.25f,
31                                0.25f, -0.25f, 0.25f,
32                                -0.25f, -0.25f, 0.25f,
33                                0.25f, 0.25f, -0.25f,
34                                -0.25f,0.25f,-0.25f,
35                                0.25f,-0.25f,-0.25f,
36                                -0.25f,-0.25f,-0.25f,
37                        };
38
39                static int[] polygons =
40                {
41                        4, 0, 2, 6, 4,
42                        4, 1, 0, 4, 5,
43                        4, 0, 1, 3, 2,
44                        4, 3, 1, 5, 7,
45                        4, 2, 3, 7, 6,
46                        4, 5, 4, 6, 7,
47                };
48
49                #endregion
50
51                const int NUM = 100;
52                const float DENSITY = 5.0f;
53                const int MAX_CONTACTS = 8;
54               
55                static IntPtr world;
56                static IntPtr space;
57                static IntPtr contactgroup;
58
59                static Queue<IntPtr> obj = new Queue<IntPtr>();
60
61                static d.Vector3 xyz = new d.Vector3(2.1640f, -1.3079f, 1.7600f);
62                static d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
63
64                static d.NearCallback nearCallback = near;
65                static d.ContactGeom[] contacts = new d.ContactGeom[MAX_CONTACTS];
66                static d.Contact contact;
67
68
69                // Called when window is opened - sets up viewpoint and prints usage
70                static void start(int unused)
71                {
72                        ds.SetViewpoint(ref xyz, ref hpr);
73                        Console.WriteLine("To drop another object, press:");
74                        Console.WriteLine("   b for box.");
75                        Console.WriteLine("   s for sphere.");
76                        Console.WriteLine("   c for capsule.");
77                        Console.WriteLine("   y for cylinder.");
78                        Console.WriteLine("   v for a convex object.");
79                        Console.WriteLine("   x for a composite object.");
80                        Console.WriteLine("To select an object, press space.");
81                        Console.WriteLine("To disable the selected object, press d.");
82                        Console.WriteLine("To enable the selected object, press e.");
83                        Console.WriteLine("To toggle showing the geom AABBs, press a.");
84                        Console.WriteLine("To toggle showing the contact points, press t.");
85                        Console.WriteLine("To toggle dropping from random position/orientation, press r.");
86                        Console.WriteLine("To save the current state to 'state.dif', press 1.");
87                }
88
89
90                // Near callback - creates contact joints
91                static void near(IntPtr space, IntPtr g1, IntPtr g2)
92                {
93                        IntPtr b1 = d.GeomGetBody(g1);
94                        IntPtr b2 = d.GeomGetBody(g2);
95                        if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
96                                return;
97
98                        int count = d.Collide(g1, g2, MAX_CONTACTS, contacts, d.ContactGeom.SizeOf);
99                        for (int i = 0; i < count; ++i)
100                        {
101                                contact.geom = contacts[i];
102                                IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact);
103                                d.JointAttach(joint, b1, b2);
104                        }
105                }
106
107
108                // Adds a new object to the scene - attaches a body to the geom and
109                // sets the initial position and orientation
110                static void addObject(IntPtr geom, d.Mass mass)
111                {
112                        // Create a body for this object
113                        IntPtr body = d.BodyCreate(world);
114                        d.GeomSetBody(geom, body);
115                        d.BodySetMass(body, ref mass);
116                        obj.Enqueue(geom);
117
118                        // Set the position of the new object
119                        d.Matrix3 R;
120                        d.BodySetPosition(body, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() + 2);
121                        d.RFromAxisAndAngle(out R, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() * 10 - 5);
122                        d.BodySetRotation(body, ref R);
123
124                        // Cap the total number of objects
125                        if (obj.Count > NUM)
126                        {
127                                geom = obj.Dequeue();
128                                body = d.GeomGetBody(geom);
129                                d.BodyDestroy(body);
130                                d.GeomDestroy(geom);
131                        }
132                }
133
134
135                // Keyboard callback
136                static void command(int cmd)
137                {
138                        IntPtr geom;
139                        d.Mass mass;
140                        d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
141
142                        Char ch = Char.ToLower((Char)cmd);
143                        switch ((Char)ch)
144                        {
145                        case 'b':
146                                d.MassSetBox(out mass, DENSITY, sides.X, sides.Y, sides.Z);
147                                geom = d.CreateBox(space, sides.X, sides.Y, sides.Z);
148                                addObject(geom, mass);
149                                break;
150
151                        case 'c':
152                                sides.X *= 0.5f;
153                                d.MassSetCapsule(out mass, DENSITY, 3, sides.X, sides.Y);
154                                geom = d.CreateCapsule(space, sides.X, sides.Y);
155                                addObject(geom, mass);
156                                break;
157
158                        case 'v':
159                                d.MassSetBox(out mass, DENSITY, 0.25f, 0.25f, 0.25f);
160                                geom = d.CreateConvex(space, planes, planes.Length / 4, points, points.Length / 3, polygons);
161                                addObject(geom, mass);
162                                break;
163                        }
164                }
165
166
167                // Draw an object in the scene
168                static void drawGeom(IntPtr geom)
169                {
170                        IntPtr body = d.GeomGetBody(geom);
171
172                        d.Vector3 pos;
173                        d.BodyCopyPosition(body, out pos);
174
175                        d.Matrix3 R;
176                        d.BodyCopyRotation(body, out R);
177
178                        d.GeomClassID type = d.GeomGetClass(geom);
179                        switch (type)
180                        {
181                        case d.GeomClassID.BoxClass:
182                                d.Vector3 sides;
183                                d.GeomBoxGetLengths(geom, out sides);
184                                ds.DrawBox(ref pos, ref R, ref sides);
185                                break;
186                        case d.GeomClassID.CapsuleClass:
187                                dReal radius, length;
188                                d.GeomCapsuleGetParams(geom, out radius, out length);
189                                ds.DrawCapsule(ref pos, ref R, length, radius);
190                                break;
191                        case d.GeomClassID.ConvexClass:
192                                ds.DrawConvex(ref pos, ref R, planes, planes.Length / 4, points, points.Length / 3, polygons);
193                                break;
194                        }
195                }
196
197
198                // Called once per frame; updates the scene
199                static void step(int pause)
200                {
201                        d.SpaceCollide(space, IntPtr.Zero, nearCallback);
202                        if (pause == 0)
203                                d.WorldQuickStep(world, 0.02f);
204                        d.JointGroupEmpty(contactgroup);
205
206                        ds.SetColor(1.0f, 1.0f, 0.0f);
207                        ds.SetTexture(ds.Texture.Wood);
208
209                        foreach (IntPtr geom in obj)
210                        {
211                                drawGeom(geom);
212                        }
213                }
214
215
216                static void Main(string[] args)
217                {
218                        // Setup pointers to drawstuff callback functions
219                        ds.Functions fn;
220                        fn.version = ds.VERSION;
221                        fn.start = new ds.CallbackFunction(start);
222                        fn.step = new ds.CallbackFunction(step);
223                        fn.command = new ds.CallbackFunction(command);
224                        fn.stop = null;
225                        fn.path_to_textures = "../../../../drawstuff/textures";
226                        if (args.Length > 0)
227                        {
228                                fn.path_to_textures = args[0];
229                        }
230
231                        // Set up contact response parameters
232                        contact.surface.mode = d.ContactFlags.Bounce | d.ContactFlags.SoftCFM;
233                        contact.surface.mu = d.Infinity;
234                        contact.surface.mu2 = 0.0f;
235                        contact.surface.bounce = 0.1f;
236                        contact.surface.bounce_vel = 0.1f;
237                        contact.surface.soft_cfm = 0.01f;
238
239                        // Initialize the scene
240                        world = d.WorldCreate();
241                        space = d.HashSpaceCreate(IntPtr.Zero);
242                        contactgroup = d.JointGroupCreate(0);
243                        d.WorldSetGravity(world, 0.0f, 0.0f, -0.5f);
244                        d.WorldSetCFM(world, 1e-5f);
245                        d.WorldSetAutoDisableFlag(world, true);
246                        d.WorldSetContactMaxCorrectingVel(world, 0.1f);
247                        d.WorldSetContactSurfaceLayer(world, 0.001f);
248                        d.CreatePlane(space, 0, 0, 1, 0);
249
250                        // Run the scene
251                        ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
252
253                        // Clean up
254                        d.JointGroupDestroy(contactgroup);
255                        d.SpaceDestroy(space);
256                        d.WorldDestroy(world);
257                        d.CloseODE();
258                }
259        }
260}
Note: See TracBrowser for help on using the repository browser.