aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/OdePlugin.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/OdePlugin.cs571
1 files changed, 571 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index c0b4b45..acd2569 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -32,6 +32,7 @@ using System.Reflection;
32using System.Runtime.InteropServices; 32using System.Runtime.InteropServices;
33using System.Threading; 33using System.Threading;
34using System.IO; 34using System.IO;
35using System.Diagnostics;
35using log4net; 36using log4net;
36using Nini.Config; 37using Nini.Config;
37using Ode.NET; 38using Ode.NET;
@@ -218,7 +219,17 @@ namespace OpenSim.Region.Physics.OdePlugin
218 private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>(); 219 private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
219 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>(); 220 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
220 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>(); 221 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
222 private bool m_NINJA_physics_joints_enabled = false;
223 //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
224 private Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
221 private d.ContactGeom[] contacts = new d.ContactGeom[80]; 225 private d.ContactGeom[] contacts = new d.ContactGeom[80];
226 private List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
227 private List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
228 private List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
229 private List<string> requestedJointsToBeDeleted = new List<string>(); // lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
230 private Object externalJointRequestsLock = new Object();
231 private Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
232 private Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
222 233
223 private d.Contact contact; 234 private d.Contact contact;
224 private d.Contact TerrainContact; 235 private d.Contact TerrainContact;
@@ -415,6 +426,9 @@ namespace OpenSim.Region.Physics.OdePlugin
415 physics_logging = physicsconfig.GetBoolean("physics_logging", false); 426 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
416 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); 427 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
417 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); 428 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
429
430 m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
431
418 } 432 }
419 } 433 }
420 434
@@ -1347,6 +1361,341 @@ namespace OpenSim.Region.Physics.OdePlugin
1347 return result; 1361 return result;
1348 } 1362 }
1349 1363
1364 public override bool SupportsNINJAJoints
1365 {
1366 get { return m_NINJA_physics_joints_enabled; }
1367 }
1368
1369 // internal utility function: must be called within a lock(OdeLock)
1370 private void InternalAddActiveJoint(PhysicsJoint joint)
1371 {
1372 activeJoints.Add(joint);
1373 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
1374 }
1375
1376 // internal utility function: must be called within a lock(OdeLock)
1377 private void InternalAddPendingJoint(OdePhysicsJoint joint)
1378 {
1379 pendingJoints.Add(joint);
1380 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
1381 }
1382
1383 // internal utility function: must be called within a lock(OdeLock)
1384 private void InternalRemovePendingJoint(PhysicsJoint joint)
1385 {
1386 pendingJoints.Remove(joint);
1387 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
1388 }
1389
1390 // internal utility function: must be called within a lock(OdeLock)
1391 private void InternalRemoveActiveJoint(PhysicsJoint joint)
1392 {
1393 activeJoints.Remove(joint);
1394 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
1395 }
1396
1397 public override void DumpJointInfo()
1398 {
1399 string hdr = "[NINJA] JOINTINFO: ";
1400 foreach (PhysicsJoint j in pendingJoints)
1401 {
1402 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1403 }
1404 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
1405 foreach (string jointName in SOPName_to_pendingJoint.Keys)
1406 {
1407 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
1408 }
1409 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
1410 foreach (PhysicsJoint j in activeJoints)
1411 {
1412 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1413 }
1414 m_log.Debug(hdr + activeJoints.Count + " total active joints");
1415 foreach (string jointName in SOPName_to_activeJoint.Keys)
1416 {
1417 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
1418 }
1419 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
1420
1421 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
1422 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
1423 foreach (string actorName in joints_connecting_actor.Keys)
1424 {
1425 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
1426 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
1427 {
1428 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1429 }
1430 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
1431 }
1432 }
1433
1434 public override void RequestJointDeletion(string ObjectNameInScene)
1435 {
1436 lock (externalJointRequestsLock)
1437 {
1438 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
1439 {
1440 requestedJointsToBeDeleted.Add(ObjectNameInScene);
1441 }
1442 }
1443 }
1444
1445 private void DeleteRequestedJoints()
1446 {
1447 List<string> myRequestedJointsToBeDeleted;
1448 lock (externalJointRequestsLock)
1449 {
1450 // make a local copy of the shared list for processing (threading issues)
1451 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
1452 }
1453
1454 foreach (string jointName in myRequestedJointsToBeDeleted)
1455 {
1456 lock (OdeLock)
1457 {
1458 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
1459 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
1460 {
1461 OdePhysicsJoint joint = null;
1462 if (SOPName_to_activeJoint.ContainsKey(jointName))
1463 {
1464 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
1465 InternalRemoveActiveJoint(joint);
1466 }
1467 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
1468 {
1469 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
1470 InternalRemovePendingJoint(joint);
1471 }
1472
1473 if (joint != null)
1474 {
1475 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
1476 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1477 {
1478 string bodyName = joint.BodyNames[iBodyName];
1479 if (bodyName != "NULL")
1480 {
1481 joints_connecting_actor[bodyName].Remove(joint);
1482 if (joints_connecting_actor[bodyName].Count == 0)
1483 {
1484 joints_connecting_actor.Remove(bodyName);
1485 }
1486 }
1487 }
1488
1489 DoJointDeactivated(joint);
1490 if (joint.jointID != IntPtr.Zero)
1491 {
1492 d.JointDestroy(joint.jointID);
1493 joint.jointID = IntPtr.Zero;
1494 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
1495 }
1496 else
1497 {
1498 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
1499 }
1500 }
1501 else
1502 {
1503 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
1504 }
1505 }
1506 else
1507 {
1508 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
1509 }
1510 }
1511 }
1512
1513 // remove processed joints from the shared list
1514 lock (externalJointRequestsLock)
1515 {
1516 foreach (string jointName in myRequestedJointsToBeDeleted)
1517 {
1518 requestedJointsToBeDeleted.Remove(jointName);
1519 }
1520 }
1521 }
1522
1523 // for pending joints we don't know if their associated bodies exist yet or not.
1524 // the joint is actually created during processing of the taints
1525 private void CreateRequestedJoints()
1526 {
1527 List<PhysicsJoint> myRequestedJointsToBeCreated;
1528 lock (externalJointRequestsLock)
1529 {
1530 // make a local copy of the shared list for processing (threading issues)
1531 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
1532 }
1533
1534 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1535 {
1536 lock (OdeLock)
1537 {
1538 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
1539 {
1540 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
1541 continue;
1542 }
1543 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
1544 {
1545 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
1546 continue;
1547 }
1548
1549 InternalAddPendingJoint(joint as OdePhysicsJoint);
1550
1551 if (joint.BodyNames.Count >= 2)
1552 {
1553 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1554 {
1555 string bodyName = joint.BodyNames[iBodyName];
1556 if (bodyName != "NULL")
1557 {
1558 if (!joints_connecting_actor.ContainsKey(bodyName))
1559 {
1560 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
1561 }
1562 joints_connecting_actor[bodyName].Add(joint);
1563 }
1564 }
1565 }
1566 }
1567 }
1568
1569 // remove processed joints from shared list
1570 lock (externalJointRequestsLock)
1571 {
1572 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1573 {
1574 requestedJointsToBeCreated.Remove(joint);
1575 }
1576 }
1577
1578 }
1579
1580 // public function to add an request for joint creation
1581 // this joint will just be added to a waiting list that is NOT processed during the main
1582 // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
1583
1584 public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, PhysicsVector position,
1585 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
1586
1587 {
1588
1589 OdePhysicsJoint joint = new OdePhysicsJoint();
1590 joint.ObjectNameInScene = objectNameInScene;
1591 joint.Type = jointType;
1592 joint.Position = new PhysicsVector(position.X, position.Y, position.Z);
1593 joint.Rotation = rotation;
1594 joint.RawParams = parms;
1595 joint.BodyNames = new List<string>(bodyNames);
1596 joint.TrackedBodyName = trackedBodyName;
1597 joint.LocalRotation = localRotation;
1598 joint.jointID = IntPtr.Zero;
1599 joint.ErrorMessageCount = 0;
1600
1601 lock (externalJointRequestsLock)
1602 {
1603 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
1604 {
1605 requestedJointsToBeCreated.Add(joint);
1606 }
1607 }
1608 return joint;
1609 }
1610
1611 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
1612 {
1613 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
1614 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
1615 {
1616
1617 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
1618 //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
1619 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
1620 {
1621 jointsToRemove.Add(j);
1622 }
1623 foreach (PhysicsJoint j in jointsToRemove)
1624 {
1625 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
1626 RequestJointDeletion(j.ObjectNameInScene);
1627 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
1628 j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
1629 }
1630 }
1631 }
1632
1633 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
1634 {
1635 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
1636 lock (OdeLock)
1637 {
1638 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
1639 RemoveAllJointsConnectedToActor(actor);
1640 }
1641 }
1642
1643 // normally called from within OnJointMoved, which is called from within a lock(OdeLock)
1644 public override PhysicsVector GetJointAnchor(PhysicsJoint joint)
1645 {
1646 Debug.Assert(joint.IsInPhysicsEngine);
1647 d.Vector3 pos = new d.Vector3();
1648
1649 if (!(joint is OdePhysicsJoint))
1650 {
1651 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
1652 }
1653 else
1654 {
1655 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
1656 switch (odeJoint.Type)
1657 {
1658 case PhysicsJointType.Ball:
1659 d.JointGetBallAnchor(odeJoint.jointID, out pos);
1660 break;
1661 case PhysicsJointType.Hinge:
1662 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
1663 break;
1664 }
1665 }
1666 return new PhysicsVector(pos.X, pos.Y, pos.Z);
1667 }
1668
1669 // normally called from within OnJointMoved, which is called from within a lock(OdeLock)
1670 // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
1671 // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
1672 // keeping track of the joint's original orientation relative to one of the involved bodies.
1673 public override PhysicsVector GetJointAxis(PhysicsJoint joint)
1674 {
1675 Debug.Assert(joint.IsInPhysicsEngine);
1676 d.Vector3 axis = new d.Vector3();
1677
1678 if (!(joint is OdePhysicsJoint))
1679 {
1680 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
1681 }
1682 else
1683 {
1684 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
1685 switch (odeJoint.Type)
1686 {
1687 case PhysicsJointType.Ball:
1688 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
1689 break;
1690 case PhysicsJointType.Hinge:
1691 d.JointGetHingeAxis(odeJoint.jointID, out axis);
1692 break;
1693 }
1694 }
1695 return new PhysicsVector(axis.X, axis.Y, axis.Z);
1696 }
1697
1698
1350 public void remActivePrim(OdePrim deactivatePrim) 1699 public void remActivePrim(OdePrim deactivatePrim)
1351 { 1700 {
1352 lock (_activeprims) 1701 lock (_activeprims)
@@ -1468,6 +1817,11 @@ namespace OpenSim.Region.Physics.OdePlugin
1468 //} 1817 //}
1469 //} 1818 //}
1470 //} 1819 //}
1820
1821 if (SupportsNINJAJoints)
1822 {
1823 RemoveAllJointsConnectedToActorThreadLocked(prim);
1824 }
1471 } 1825 }
1472 } 1826 }
1473 } 1827 }
@@ -1873,6 +2227,13 @@ namespace OpenSim.Region.Physics.OdePlugin
1873 { 2227 {
1874 m_physicsiterations = 10; 2228 m_physicsiterations = 10;
1875 } 2229 }
2230
2231 if (SupportsNINJAJoints)
2232 {
2233 DeleteRequestedJoints(); // this must be outside of the lock(OdeLock) to avoid deadlocks
2234 CreateRequestedJoints(); // this must be outside of the lock(OdeLock) to avoid deadlocks
2235 }
2236
1876 lock (OdeLock) 2237 lock (OdeLock)
1877 { 2238 {
1878 // Process 10 frames if the sim is running normal.. 2239 // Process 10 frames if the sim is running normal..
@@ -1944,6 +2305,188 @@ namespace OpenSim.Region.Physics.OdePlugin
1944 prim.m_collisionscore = 0; 2305 prim.m_collisionscore = 0;
1945 } 2306 }
1946 2307
2308 if (SupportsNINJAJoints)
2309 {
2310 // Create pending joints, if possible
2311
2312 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2313 // a joint requires specifying the body id of both involved bodies
2314 if (pendingJoints.Count > 0)
2315 {
2316 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
2317 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
2318 foreach (PhysicsJoint joint in pendingJoints)
2319 {
2320 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
2321 string[] jointParams = joint.RawParams.Split(' ');
2322 List<IntPtr> jointBodies = new List<IntPtr>();
2323 bool allJointBodiesAreReady = true;
2324 foreach (string jointParam in jointParams)
2325 {
2326 if (jointParam == "NULL")
2327 {
2328 //DoJointErrorMessage(joint, "attaching NULL joint to world");
2329 jointBodies.Add(IntPtr.Zero);
2330 }
2331 else
2332 {
2333 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
2334 bool foundPrim = false;
2335 lock (_prims)
2336 {
2337 foreach (OdePrim prim in _prims) // FIXME: inefficient
2338 {
2339 if (prim.SOPName == jointParam)
2340 {
2341 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
2342 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
2343 {
2344 jointBodies.Add(prim.Body);
2345 foundPrim = true;
2346 break;
2347 }
2348 else
2349 {
2350 DoJointErrorMessage(joint, "prim name " + jointParam +
2351 " exists but is not (yet) physical; deferring joint creation. " +
2352 "IsPhysical property is " + prim.IsPhysical +
2353 " and body is " + prim.Body);
2354 foundPrim = false;
2355 break;
2356 }
2357 }
2358 }
2359 }
2360 if (foundPrim)
2361 {
2362 // all is fine
2363 }
2364 else
2365 {
2366 allJointBodiesAreReady = false;
2367 break;
2368 }
2369 }
2370 }
2371 if (allJointBodiesAreReady)
2372 {
2373 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
2374 if (jointBodies[0] == jointBodies[1])
2375 {
2376 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
2377 }
2378 else
2379 {
2380 switch (joint.Type)
2381 {
2382 case PhysicsJointType.Ball:
2383 {
2384 IntPtr odeJoint;
2385 //DoJointErrorMessage(joint, "ODE creating ball joint ");
2386 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
2387 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2388 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2389 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
2390 d.JointSetBallAnchor(odeJoint,
2391 joint.Position.X,
2392 joint.Position.Y,
2393 joint.Position.Z);
2394 //DoJointErrorMessage(joint, "ODE joint setting OK");
2395 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
2396 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
2397 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
2398 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
2399
2400 if (joint is OdePhysicsJoint)
2401 {
2402 ((OdePhysicsJoint)joint).jointID = odeJoint;
2403 }
2404 else
2405 {
2406 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2407 }
2408 }
2409 break;
2410 case PhysicsJointType.Hinge:
2411 {
2412 IntPtr odeJoint;
2413 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
2414 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
2415 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2416 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2417 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
2418 d.JointSetHingeAnchor(odeJoint,
2419 joint.Position.X,
2420 joint.Position.Y,
2421 joint.Position.Z);
2422 // We use the orientation of the x-axis of the joint's coordinate frame
2423 // as the axis for the hinge.
2424
2425 // Therefore, we must get the joint's coordinate frame based on the
2426 // joint.Rotation field, which originates from the orientation of the
2427 // joint's proxy object in the scene.
2428
2429 // The joint's coordinate frame is defined as the transformation matrix
2430 // that converts a vector from joint-local coordinates into world coordinates.
2431 // World coordinates are defined as the XYZ coordinate system of the sim,
2432 // as shown in the top status-bar of the viewer.
2433
2434 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
2435 // and use that as the hinge axis.
2436
2437 //joint.Rotation.Normalize();
2438 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
2439
2440 // Now extract the X axis of the joint's coordinate frame.
2441
2442 // Do not try to use proxyFrame.AtAxis or you will become mired in the
2443 // tar pit of transposed, inverted, and generally messed-up orientations.
2444 // (In other words, Matrix4.AtAxis() is borked.)
2445 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
2446
2447 // Instead, compute the X axis of the coordinate frame by transforming
2448 // the (1,0,0) vector. At least that works.
2449
2450 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
2451 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
2452 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
2453 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
2454 d.JointSetHingeAxis(odeJoint,
2455 jointAxis.X,
2456 jointAxis.Y,
2457 jointAxis.Z);
2458 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
2459 if (joint is OdePhysicsJoint)
2460 {
2461 ((OdePhysicsJoint)joint).jointID = odeJoint;
2462 }
2463 else
2464 {
2465 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2466 }
2467 }
2468 break;
2469 }
2470 successfullyProcessedPendingJoints.Add(joint);
2471 }
2472 }
2473 else
2474 {
2475 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
2476 }
2477 }
2478 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
2479 {
2480 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
2481 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
2482 InternalRemovePendingJoint(successfullyProcessedJoint);
2483 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
2484 InternalAddActiveJoint(successfullyProcessedJoint);
2485 //DoJointErrorMessage(successfullyProcessedJoint, "done");
2486 }
2487 }
2488 }
2489
1947 if (processedtaints) 2490 if (processedtaints)
1948 _taintedPrim.Clear(); 2491 _taintedPrim.Clear();
1949 } 2492 }
@@ -2032,11 +2575,39 @@ namespace OpenSim.Region.Physics.OdePlugin
2032 if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag)) 2575 if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
2033 { 2576 {
2034 actor.UpdatePositionAndVelocity(); 2577 actor.UpdatePositionAndVelocity();
2578
2579 if (SupportsNINJAJoints)
2580 {
2581 // If an actor moved, move its joint proxy objects as well.
2582 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
2583 // for this purpose but it is never called! So we just do the joint
2584 // movement code here.
2585
2586 if (actor.SOPName != null &&
2587 joints_connecting_actor.ContainsKey(actor.SOPName) &&
2588 joints_connecting_actor[actor.SOPName] != null &&
2589 joints_connecting_actor[actor.SOPName].Count > 0)
2590 {
2591 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
2592 {
2593 if (affectedJoint.IsInPhysicsEngine)
2594 {
2595 DoJointMoved(affectedJoint);
2596 }
2597 else
2598 {
2599 DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
2600 }
2601 }
2602 }
2603 }
2035 } 2604 }
2036 } 2605 }
2037 } 2606 }
2038 } 2607 }
2039 2608
2609 //DumpJointInfo();
2610
2040 // Finished with all sim stepping. If requested, dump world state to file for debugging. 2611 // Finished with all sim stepping. If requested, dump world state to file for debugging.
2041 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? 2612 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
2042 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? 2613 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
generated by cgit v1.1 at 2024-11-10 02:40:23 +0000