aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/OpenSim.Physics/OdePlugin/OdePlugin.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/OpenSim.Physics/OdePlugin/OdePlugin.cs')
-rw-r--r--OpenSim/OpenSim.Physics/OdePlugin/OdePlugin.cs452
1 files changed, 452 insertions, 0 deletions
diff --git a/OpenSim/OpenSim.Physics/OdePlugin/OdePlugin.cs b/OpenSim/OpenSim.Physics/OdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..599dea8
--- /dev/null
+++ b/OpenSim/OpenSim.Physics/OdePlugin/OdePlugin.cs
@@ -0,0 +1,452 @@
1/*
2* Copyright (c) OpenSim project, http://sim.opensecondlife.org/
3*
4* Redistribution and use in source and binary forms, with or without
5* modification, are permitted provided that the following conditions are met:
6* * Redistributions of source code must retain the above copyright
7* notice, this list of conditions and the following disclaimer.
8* * Redistributions in binary form must reproduce the above copyright
9* notice, this list of conditions and the following disclaimer in the
10* documentation and/or other materials provided with the distribution.
11* * Neither the name of the <organization> nor the
12* names of its contributors may be used to endorse or promote products
13* derived from this software without specific prior written permission.
14*
15* THIS SOFTWARE IS PROVIDED BY <copyright holder> ``AS IS'' AND ANY
16* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18* DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY
19* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25*
26*/
27using System;
28using System.Collections.Generic;
29using OpenSim.Physics.Manager;
30using Ode.NET;
31
32namespace OpenSim.Physics.OdePlugin
33{
34 /// <summary>
35 /// ODE plugin
36 /// </summary>
37 public class OdePlugin : IPhysicsPlugin
38 {
39 private OdeScene _mScene;
40
41 public OdePlugin()
42 {
43
44 }
45
46 public bool Init()
47 {
48 return true;
49 }
50
51 public PhysicsScene GetScene()
52 {
53 if (_mScene == null)
54 {
55 _mScene = new OdeScene();
56 }
57 return (_mScene);
58 }
59
60 public string GetName()
61 {
62 return ("OpenDynamicsEngine");
63 }
64
65 public void Dispose()
66 {
67
68 }
69 }
70
71 public class OdeScene : PhysicsScene
72 {
73 static public IntPtr world;
74 static public IntPtr space;
75 static private IntPtr contactgroup;
76 static private IntPtr LandGeom;
77 //static private IntPtr Land;
78 private double[] _heightmap;
79 static private d.NearCallback nearCallback = near;
80 private List<OdeCharacter> _characters = new List<OdeCharacter>();
81 private static d.ContactGeom[] contacts = new d.ContactGeom[30];
82 private static d.Contact contact;
83
84 public OdeScene()
85 {
86 contact.surface.mode = d.ContactFlags.Bounce | d.ContactFlags.SoftCFM;
87 contact.surface.mu = d.Infinity;
88 contact.surface.mu2 = 0.0f;
89 contact.surface.bounce = 0.1f;
90 contact.surface.bounce_vel = 0.1f;
91 contact.surface.soft_cfm = 0.01f;
92
93 world = d.WorldCreate();
94 space = d.HashSpaceCreate(IntPtr.Zero);
95 contactgroup = d.JointGroupCreate(0);
96 d.WorldSetGravity(world, 0.0f, 0.0f, -0.5f);
97 //d.WorldSetCFM(world, 1e-5f);
98 d.WorldSetAutoDisableFlag(world, false);
99 d.WorldSetContactSurfaceLayer(world, 0.001f);
100 // d.CreatePlane(space, 0, 0, 1, 0);
101 this._heightmap = new double[65536];
102 }
103
104 // This function blatantly ripped off from BoxStack.cs
105 static private void near(IntPtr space, IntPtr g1, IntPtr g2)
106 {
107 //Console.WriteLine("collision callback");
108 IntPtr b1 = d.GeomGetBody(g1);
109 IntPtr b2 = d.GeomGetBody(g2);
110 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
111 return;
112
113 int count = d.Collide(g1, g2, 500, contacts, d.ContactGeom.SizeOf);
114 for (int i = 0; i < count; ++i)
115 {
116 contact.geom = contacts[i];
117 IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact);
118 d.JointAttach(joint, b1, b2);
119 }
120
121 }
122
123 public override PhysicsActor AddAvatar(PhysicsVector position)
124 {
125 PhysicsVector pos = new PhysicsVector();
126 pos.X = position.X;
127 pos.Y = position.Y;
128 pos.Z = position.Z + 20;
129 OdeCharacter newAv = new OdeCharacter(this, pos);
130 this._characters.Add(newAv);
131 return newAv;
132 }
133
134 public override void RemoveAvatar(PhysicsActor actor)
135 {
136
137 }
138
139 public override PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size)
140 {
141 PhysicsVector pos = new PhysicsVector();
142 pos.X = position.X;
143 pos.Y = position.Y;
144 pos.Z = position.Z;
145 PhysicsVector siz = new PhysicsVector();
146 siz.X = size.X;
147 siz.Y = size.Y;
148 siz.Z = size.Z;
149 return new OdePrim();
150 }
151
152 public override void Simulate(float timeStep)
153 {
154 foreach (OdeCharacter actor in _characters)
155 {
156 actor.Move(timeStep * 5f);
157 }
158 d.SpaceCollide(space, IntPtr.Zero, nearCallback);
159 d.WorldQuickStep(world, timeStep * 5f);
160 d.JointGroupEmpty(contactgroup);
161 foreach (OdeCharacter actor in _characters)
162 {
163 actor.UpdatePosition();
164 }
165
166 }
167
168 public override void GetResults()
169 {
170
171 }
172
173 public override bool IsThreaded
174 {
175 get
176 {
177 return (false); // for now we won't be multithreaded
178 }
179 }
180
181 public override void SetTerrain(float[] heightMap)
182 {
183 for (int i = 0; i < 65536; i++)
184 {
185 this._heightmap[i] = (double)heightMap[i];
186 }
187 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
188 d.GeomHeightfieldDataBuildDouble(HeightmapData, _heightmap, 0, 256, 256, 256, 256, 1.0f, 0.0f, 2.0f, 0);
189 d.GeomHeightfieldDataSetBounds(HeightmapData, 256, 256);
190 LandGeom = d.CreateHeightfield(space, HeightmapData, 1);
191 d.Matrix3 R = new d.Matrix3();
192
193 Axiom.MathLib.Quaternion q1 =Axiom.MathLib.Quaternion.FromAngleAxis(1.5707f, new Axiom.MathLib.Vector3(1,0,0));
194 Axiom.MathLib.Quaternion q2 =Axiom.MathLib.Quaternion.FromAngleAxis(1.5707f, new Axiom.MathLib.Vector3(0,1,0));
195 //Axiom.MathLib.Quaternion q3 = Axiom.MathLib.Quaternion.FromAngleAxis(3.14f, new Axiom.MathLib.Vector3(0, 0, 1));
196
197 q1 = q1 * q2;
198 //q1 = q1 * q3;
199 Axiom.MathLib.Vector3 v3 = new Axiom.MathLib.Vector3();
200 float angle = 0;
201 q1.ToAngleAxis(ref angle, ref v3);
202
203 d.RFromAxisAndAngle(out R, v3.x, v3.y, v3.z, angle);
204 d.GeomSetRotation(LandGeom, ref R);
205 d.GeomSetPosition(LandGeom, 128, 128, 0);
206 }
207
208 public override void DeleteTerrain()
209 {
210
211 }
212 }
213
214 public class OdeCharacter : PhysicsActor
215 {
216 private PhysicsVector _position;
217 private PhysicsVector _velocity;
218 private PhysicsVector _acceleration;
219 private bool flying;
220 //private float gravityAccel;
221 private IntPtr BoundingCapsule;
222 IntPtr capsule_geom;
223 d.Mass capsule_mass;
224
225 public OdeCharacter(OdeScene parent_scene, PhysicsVector pos)
226 {
227 _velocity = new PhysicsVector();
228 _position = pos;
229 _acceleration = new PhysicsVector();
230 d.MassSetCapsule(out capsule_mass, 5.0f, 3, 0.5f, 2f);
231 capsule_geom = d.CreateCapsule(OdeScene.space, 0.5f, 2f);
232 this.BoundingCapsule = d.BodyCreate(OdeScene.world);
233 d.BodySetMass(BoundingCapsule, ref capsule_mass);
234 d.BodySetPosition(BoundingCapsule, pos.X, pos.Y, pos.Z);
235 d.GeomSetBody(capsule_geom, BoundingCapsule);
236 }
237
238 public override bool Flying
239 {
240 get
241 {
242 return flying;
243 }
244 set
245 {
246 flying = value;
247 }
248 }
249
250 public override PhysicsVector Position
251 {
252 get
253 {
254 return _position;
255 }
256 set
257 {
258 _position = value;
259 }
260 }
261
262 public override PhysicsVector Velocity
263 {
264 get
265 {
266 return _velocity;
267 }
268 set
269 {
270 _velocity = value;
271 }
272 }
273
274 public override bool Kinematic
275 {
276 get
277 {
278 return false;
279 }
280 set
281 {
282
283 }
284 }
285
286 public override Axiom.MathLib.Quaternion Orientation
287 {
288 get
289 {
290 return Axiom.MathLib.Quaternion.Identity;
291 }
292 set
293 {
294
295 }
296 }
297
298 public override PhysicsVector Acceleration
299 {
300 get
301 {
302 return _acceleration;
303 }
304
305 }
306 public void SetAcceleration(PhysicsVector accel)
307 {
308 this._acceleration = accel;
309 }
310
311 public override void AddForce(PhysicsVector force)
312 {
313
314 }
315
316 public override void SetMomentum(PhysicsVector momentum)
317 {
318
319 }
320
321 public void Move(float timeStep)
322 {
323 PhysicsVector vec = new PhysicsVector();
324 vec.X = this._velocity.X * timeStep;
325 vec.Y = this._velocity.Y * timeStep;
326 if (flying)
327 {
328 vec.Z = (this._velocity.Z + 0.5f) * timeStep;
329 }
330 d.BodySetLinearVel(this.BoundingCapsule, vec.X, vec.Y, vec.Z);
331 }
332
333 public void UpdatePosition()
334 {
335 d.Vector3 vec = d.BodyGetPosition(BoundingCapsule);
336 this._position.X = vec.X;
337 this._position.Y = vec.Y;
338 this._position.Z = vec.Z;
339 }
340 }
341
342 public class OdePrim : PhysicsActor
343 {
344 private PhysicsVector _position;
345 private PhysicsVector _velocity;
346 private PhysicsVector _acceleration;
347
348 public OdePrim()
349 {
350 _velocity = new PhysicsVector();
351 _position = new PhysicsVector();
352 _acceleration = new PhysicsVector();
353 }
354 public override bool Flying
355 {
356 get
357 {
358 return false; //no flying prims for you
359 }
360 set
361 {
362
363 }
364 }
365 public override PhysicsVector Position
366 {
367 get
368 {
369 PhysicsVector pos = new PhysicsVector();
370 // PhysicsVector vec = this._prim.Position;
371 //pos.X = vec.X;
372 //pos.Y = vec.Y;
373 //pos.Z = vec.Z;
374 return pos;
375
376 }
377 set
378 {
379 /*PhysicsVector vec = value;
380 PhysicsVector pos = new PhysicsVector();
381 pos.X = vec.X;
382 pos.Y = vec.Y;
383 pos.Z = vec.Z;
384 this._prim.Position = pos;*/
385 }
386 }
387
388 public override PhysicsVector Velocity
389 {
390 get
391 {
392 return _velocity;
393 }
394 set
395 {
396 _velocity = value;
397 }
398 }
399
400 public override bool Kinematic
401 {
402 get
403 {
404 return false;
405 //return this._prim.Kinematic;
406 }
407 set
408 {
409 //this._prim.Kinematic = value;
410 }
411 }
412
413 public override Axiom.MathLib.Quaternion Orientation
414 {
415 get
416 {
417 Axiom.MathLib.Quaternion res = new Axiom.MathLib.Quaternion();
418 return res;
419 }
420 set
421 {
422
423 }
424 }
425
426 public override PhysicsVector Acceleration
427 {
428 get
429 {
430 return _acceleration;
431 }
432
433 }
434 public void SetAcceleration(PhysicsVector accel)
435 {
436 this._acceleration = accel;
437 }
438
439 public override void AddForce(PhysicsVector force)
440 {
441
442 }
443
444 public override void SetMomentum(PhysicsVector momentum)
445 {
446
447 }
448
449
450 }
451
452}
generated by cgit v1.1 at 2024-11-05 01:11:02 +0000