aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
diff options
context:
space:
mode:
authorJustin Clark-Casey (justincc)2011-07-29 00:39:02 +0100
committerJustin Clark-Casey (justincc)2011-07-29 00:39:02 +0100
commit3f0d8f3cbf0b340848aa938b83638567ba8b02cf (patch)
tree4217b45e0422f1addae9a985c73ac71eb90e31f6 /OpenSim/Region/Physics/OdePlugin/OdeScene.cs
parentrefactor: unindent the OdeScene.Simulate() loop to ignore the long commented ... (diff)
downloadopensim-SC_OLD-3f0d8f3cbf0b340848aa938b83638567ba8b02cf.zip
opensim-SC_OLD-3f0d8f3cbf0b340848aa938b83638567ba8b02cf.tar.gz
opensim-SC_OLD-3f0d8f3cbf0b340848aa938b83638567ba8b02cf.tar.bz2
opensim-SC_OLD-3f0d8f3cbf0b340848aa938b83638567ba8b02cf.tar.xz
refactor: Simplify reading OdeScene.Simulate() loop by shunting all the NINJA joints stuff into its own method.
Now if ninja joints isn't active (which is the default) don't have to wade through a lot of massively indented irrelevant code.
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/OdeScene.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/OdeScene.cs373
1 files changed, 191 insertions, 182 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index a6d737e..9d41b15 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -2732,192 +2732,13 @@ Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.Name);
2732 2732
2733 // This loop can block up the Heartbeat for a very long time on large regions. 2733 // This loop can block up the Heartbeat for a very long time on large regions.
2734 // We need to let the Watchdog know that the Heartbeat is not dead 2734 // We need to let the Watchdog know that the Heartbeat is not dead
2735 // NOTE: This is currently commented out, but if things like OAR loading are 2735 // NOTE: This is currently commented out, but if things like OAR loading are
2736 // timing the heartbeat out we will need to uncomment it 2736 // timing the heartbeat out we will need to uncomment it
2737 //Watchdog.UpdateThread(); 2737 //Watchdog.UpdateThread();
2738 } 2738 }
2739 2739
2740 if (SupportsNINJAJoints) 2740 if (SupportsNINJAJoints)
2741 { 2741 SimulateNINJAJoints();
2742 // Create pending joints, if possible
2743
2744 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2745 // a joint requires specifying the body id of both involved bodies
2746 if (pendingJoints.Count > 0)
2747 {
2748 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
2749 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
2750 foreach (PhysicsJoint joint in pendingJoints)
2751 {
2752 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
2753 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
2754 List<IntPtr> jointBodies = new List<IntPtr>();
2755 bool allJointBodiesAreReady = true;
2756 foreach (string jointParam in jointParams)
2757 {
2758 if (jointParam == "NULL")
2759 {
2760 //DoJointErrorMessage(joint, "attaching NULL joint to world");
2761 jointBodies.Add(IntPtr.Zero);
2762 }
2763 else
2764 {
2765 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
2766 bool foundPrim = false;
2767 lock (_prims)
2768 {
2769 foreach (OdePrim prim in _prims) // FIXME: inefficient
2770 {
2771 if (prim.SOPName == jointParam)
2772 {
2773 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
2774 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
2775 {
2776 jointBodies.Add(prim.Body);
2777 foundPrim = true;
2778 break;
2779 }
2780 else
2781 {
2782 DoJointErrorMessage(joint, "prim name " + jointParam +
2783 " exists but is not (yet) physical; deferring joint creation. " +
2784 "IsPhysical property is " + prim.IsPhysical +
2785 " and body is " + prim.Body);
2786 foundPrim = false;
2787 break;
2788 }
2789 }
2790 }
2791 }
2792 if (foundPrim)
2793 {
2794 // all is fine
2795 }
2796 else
2797 {
2798 allJointBodiesAreReady = false;
2799 break;
2800 }
2801 }
2802 }
2803 if (allJointBodiesAreReady)
2804 {
2805 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
2806 if (jointBodies[0] == jointBodies[1])
2807 {
2808 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
2809 }
2810 else
2811 {
2812 switch (joint.Type)
2813 {
2814 case PhysicsJointType.Ball:
2815 {
2816 IntPtr odeJoint;
2817 //DoJointErrorMessage(joint, "ODE creating ball joint ");
2818 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
2819 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2820 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2821 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
2822 d.JointSetBallAnchor(odeJoint,
2823 joint.Position.X,
2824 joint.Position.Y,
2825 joint.Position.Z);
2826 //DoJointErrorMessage(joint, "ODE joint setting OK");
2827 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
2828 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
2829 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
2830 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
2831
2832 if (joint is OdePhysicsJoint)
2833 {
2834 ((OdePhysicsJoint)joint).jointID = odeJoint;
2835 }
2836 else
2837 {
2838 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2839 }
2840 }
2841 break;
2842 case PhysicsJointType.Hinge:
2843 {
2844 IntPtr odeJoint;
2845 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
2846 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
2847 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2848 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2849 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
2850 d.JointSetHingeAnchor(odeJoint,
2851 joint.Position.X,
2852 joint.Position.Y,
2853 joint.Position.Z);
2854 // We use the orientation of the x-axis of the joint's coordinate frame
2855 // as the axis for the hinge.
2856
2857 // Therefore, we must get the joint's coordinate frame based on the
2858 // joint.Rotation field, which originates from the orientation of the
2859 // joint's proxy object in the scene.
2860
2861 // The joint's coordinate frame is defined as the transformation matrix
2862 // that converts a vector from joint-local coordinates into world coordinates.
2863 // World coordinates are defined as the XYZ coordinate system of the sim,
2864 // as shown in the top status-bar of the viewer.
2865
2866 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
2867 // and use that as the hinge axis.
2868
2869 //joint.Rotation.Normalize();
2870 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
2871
2872 // Now extract the X axis of the joint's coordinate frame.
2873
2874 // Do not try to use proxyFrame.AtAxis or you will become mired in the
2875 // tar pit of transposed, inverted, and generally messed-up orientations.
2876 // (In other words, Matrix4.AtAxis() is borked.)
2877 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
2878
2879 // Instead, compute the X axis of the coordinate frame by transforming
2880 // the (1,0,0) vector. At least that works.
2881
2882 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
2883 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
2884 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
2885 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
2886 d.JointSetHingeAxis(odeJoint,
2887 jointAxis.X,
2888 jointAxis.Y,
2889 jointAxis.Z);
2890 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
2891 if (joint is OdePhysicsJoint)
2892 {
2893 ((OdePhysicsJoint)joint).jointID = odeJoint;
2894 }
2895 else
2896 {
2897 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2898 }
2899 }
2900 break;
2901 }
2902 successfullyProcessedPendingJoints.Add(joint);
2903 }
2904 }
2905 else
2906 {
2907 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
2908 }
2909 }
2910 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
2911 {
2912 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
2913 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
2914 InternalRemovePendingJoint(successfullyProcessedJoint);
2915 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
2916 InternalAddActiveJoint(successfullyProcessedJoint);
2917 //DoJointErrorMessage(successfullyProcessedJoint, "done");
2918 }
2919 }
2920 }
2921 2742
2922 if (processedtaints) 2743 if (processedtaints)
2923//Console.WriteLine("Simulate calls Clear of _taintedPrim list"); 2744//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
@@ -3095,7 +2916,7 @@ Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.Name);
3095 2916
3096 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); 2917 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
3097 } 2918 }
3098 2919
3099 latertickcount = Util.EnvironmentTickCount() - tickCountFrameRun; 2920 latertickcount = Util.EnvironmentTickCount() - tickCountFrameRun;
3100 2921
3101 // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics 2922 // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics
@@ -3117,6 +2938,194 @@ Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.Name);
3117 return fps; 2938 return fps;
3118 } 2939 }
3119 2940
2941 /// <summary>
2942 /// Simulate NINJA joints.
2943 /// </summary>
2944 /// <remarks>
2945 /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else.
2946 /// </remarks>
2947 protected void SimulateNINJAJoints()
2948 {
2949 // Create pending joints, if possible
2950
2951 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2952 // a joint requires specifying the body id of both involved bodies
2953 if (pendingJoints.Count > 0)
2954 {
2955 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
2956 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
2957 foreach (PhysicsJoint joint in pendingJoints)
2958 {
2959 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
2960 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
2961 List<IntPtr> jointBodies = new List<IntPtr>();
2962 bool allJointBodiesAreReady = true;
2963 foreach (string jointParam in jointParams)
2964 {
2965 if (jointParam == "NULL")
2966 {
2967 //DoJointErrorMessage(joint, "attaching NULL joint to world");
2968 jointBodies.Add(IntPtr.Zero);
2969 }
2970 else
2971 {
2972 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
2973 bool foundPrim = false;
2974 lock (_prims)
2975 {
2976 foreach (OdePrim prim in _prims) // FIXME: inefficient
2977 {
2978 if (prim.SOPName == jointParam)
2979 {
2980 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
2981 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
2982 {
2983 jointBodies.Add(prim.Body);
2984 foundPrim = true;
2985 break;
2986 }
2987 else
2988 {
2989 DoJointErrorMessage(joint, "prim name " + jointParam +
2990 " exists but is not (yet) physical; deferring joint creation. " +
2991 "IsPhysical property is " + prim.IsPhysical +
2992 " and body is " + prim.Body);
2993 foundPrim = false;
2994 break;
2995 }
2996 }
2997 }
2998 }
2999 if (foundPrim)
3000 {
3001 // all is fine
3002 }
3003 else
3004 {
3005 allJointBodiesAreReady = false;
3006 break;
3007 }
3008 }
3009 }
3010 if (allJointBodiesAreReady)
3011 {
3012 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
3013 if (jointBodies[0] == jointBodies[1])
3014 {
3015 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
3016 }
3017 else
3018 {
3019 switch (joint.Type)
3020 {
3021 case PhysicsJointType.Ball:
3022 {
3023 IntPtr odeJoint;
3024 //DoJointErrorMessage(joint, "ODE creating ball joint ");
3025 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
3026 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3027 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3028 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
3029 d.JointSetBallAnchor(odeJoint,
3030 joint.Position.X,
3031 joint.Position.Y,
3032 joint.Position.Z);
3033 //DoJointErrorMessage(joint, "ODE joint setting OK");
3034 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
3035 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
3036 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
3037 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
3038
3039 if (joint is OdePhysicsJoint)
3040 {
3041 ((OdePhysicsJoint)joint).jointID = odeJoint;
3042 }
3043 else
3044 {
3045 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3046 }
3047 }
3048 break;
3049 case PhysicsJointType.Hinge:
3050 {
3051 IntPtr odeJoint;
3052 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
3053 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
3054 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3055 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3056 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
3057 d.JointSetHingeAnchor(odeJoint,
3058 joint.Position.X,
3059 joint.Position.Y,
3060 joint.Position.Z);
3061 // We use the orientation of the x-axis of the joint's coordinate frame
3062 // as the axis for the hinge.
3063
3064 // Therefore, we must get the joint's coordinate frame based on the
3065 // joint.Rotation field, which originates from the orientation of the
3066 // joint's proxy object in the scene.
3067
3068 // The joint's coordinate frame is defined as the transformation matrix
3069 // that converts a vector from joint-local coordinates into world coordinates.
3070 // World coordinates are defined as the XYZ coordinate system of the sim,
3071 // as shown in the top status-bar of the viewer.
3072
3073 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
3074 // and use that as the hinge axis.
3075
3076 //joint.Rotation.Normalize();
3077 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
3078
3079 // Now extract the X axis of the joint's coordinate frame.
3080
3081 // Do not try to use proxyFrame.AtAxis or you will become mired in the
3082 // tar pit of transposed, inverted, and generally messed-up orientations.
3083 // (In other words, Matrix4.AtAxis() is borked.)
3084 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
3085
3086 // Instead, compute the X axis of the coordinate frame by transforming
3087 // the (1,0,0) vector. At least that works.
3088
3089 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
3090 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
3091 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
3092 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
3093 d.JointSetHingeAxis(odeJoint,
3094 jointAxis.X,
3095 jointAxis.Y,
3096 jointAxis.Z);
3097 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
3098 if (joint is OdePhysicsJoint)
3099 {
3100 ((OdePhysicsJoint)joint).jointID = odeJoint;
3101 }
3102 else
3103 {
3104 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3105 }
3106 }
3107 break;
3108 }
3109 successfullyProcessedPendingJoints.Add(joint);
3110 }
3111 }
3112 else
3113 {
3114 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
3115 }
3116 }
3117 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
3118 {
3119 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
3120 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
3121 InternalRemovePendingJoint(successfullyProcessedJoint);
3122 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
3123 InternalAddActiveJoint(successfullyProcessedJoint);
3124 //DoJointErrorMessage(successfullyProcessedJoint, "done");
3125 }
3126 }
3127 }
3128
3120 public override void GetResults() 3129 public override void GetResults()
3121 { 3130 {
3122 } 3131 }
generated by cgit v1.1 at 2024-11-08 19:01:42 +0000