aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs124
1 files changed, 120 insertions, 4 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index ddadc98..43a7272 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -46,11 +46,17 @@ namespace OpenSim.Region.Physics.OdePlugin
46 private PhysicsVector m_rotationalVelocity; 46 private PhysicsVector m_rotationalVelocity;
47 private PhysicsVector _size; 47 private PhysicsVector _size;
48 private PhysicsVector _acceleration; 48 private PhysicsVector _acceleration;
49 private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
49 private Quaternion _orientation; 50 private Quaternion _orientation;
50 private PhysicsVector m_taintposition; 51 private PhysicsVector m_taintposition;
51 private PhysicsVector m_taintsize; 52 private PhysicsVector m_taintsize;
52 private PhysicsVector m_taintVelocity = PhysicsVector.Zero; 53 private PhysicsVector m_taintVelocity = PhysicsVector.Zero;
53 private Quaternion m_taintrot; 54 private Quaternion m_taintrot;
55
56 private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0);
57 private float m_PIDTau = 0f;
58 private bool m_usePID = false;
59
54 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom 60 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
55 | CollisionCategories.Space 61 | CollisionCategories.Space
56 | CollisionCategories.Body 62 | CollisionCategories.Body
@@ -1079,31 +1085,138 @@ namespace OpenSim.Region.Physics.OdePlugin
1079 1085
1080 public void Move(float timestep) 1086 public void Move(float timestep)
1081 { 1087 {
1088 float fx = 0;
1089 float fy = 0;
1090 float fz = 0;
1082 1091
1083 if (IsPhysical && Body != (IntPtr)0 && !m_isSelected) 1092 if (IsPhysical && Body != (IntPtr)0 && !m_isSelected)
1084 { 1093 {
1094 float PID_D = 2200.0f;
1095 float PID_P = 900.0f;
1096
1097
1085 float m_mass = CalculateMass(); 1098 float m_mass = CalculateMass();
1099
1100 fz = 0f;
1086 //m_log.Info(m_collisionFlags.ToString()); 1101 //m_log.Info(m_collisionFlags.ToString());
1102
1103
1104
1105
1087 if (m_buoyancy != 0) 1106 if (m_buoyancy != 0)
1088 { 1107 {
1089 float buoyancy = 0f; 1108
1090 if (m_buoyancy > 0) 1109 if (m_buoyancy > 0)
1091 { 1110 {
1092 buoyancy = ((9.8f * m_buoyancy) * m_mass); 1111 fz = ((9.8f * m_buoyancy) * m_mass);
1093 1112
1094 //d.Vector3 l_velocity = d.BodyGetLinearVel(Body); 1113 //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
1095 //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (9.8f * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString()); 1114 //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (9.8f * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString());
1096 } 1115 }
1097 else 1116 else
1098 { 1117 {
1099 buoyancy = (-1 * ((9.8f * (-1 * m_buoyancy)) * m_mass)); 1118 fz = (-1 * ((9.8f * (-1 * m_buoyancy)) * m_mass));
1119 }
1120
1121
1122 }
1123
1124 if (m_usePID)
1125 {
1126
1127 // If we're using the PID controller, then we have no gravity
1128 fz = ((9.8f) * this.Mass );
1129
1130 // no lock; for now it's only called from within Simulate()
1131
1132 // If the PID Controller isn't active then we set our force
1133 // calculating base velocity to the current position
1134 if (System.Environment.OSVersion.Platform == PlatformID.Unix)
1135 {
1136 PID_D = 3200.0f;
1137 PID_P = 1400.0f;
1138 }
1139 else
1140 {
1141 PID_D = 2200.0f;
1142 PID_P = 900.0f;
1143 }
1144 PID_D = 1.0f;
1145 PID_P = 1.0f;
1146
1147
1148 //PidStatus = true;
1149
1150 PhysicsVector vec = new PhysicsVector();
1151 d.Vector3 vel = d.BodyGetLinearVel(Body);
1152
1153
1154 d.Vector3 pos = d.BodyGetPosition(Body);
1155 _target_velocity =
1156 new PhysicsVector(
1157 (m_PIDTarget.X - pos.X) / m_PIDTau,
1158 (m_PIDTarget.Y - pos.Y) / m_PIDTau,
1159 (m_PIDTarget.Z - pos.Z) / m_PIDTau
1160 );
1161
1162
1163 // if velocity is zero, use position control; otherwise, velocity control
1164
1165 if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f))
1166 {
1167 // keep track of where we stopped. No more slippin' & slidin'
1168
1169
1170 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1171 // react to the physics scene by moving it's position.
1172 // Avatar to Avatar collisions
1173 // Prim to avatar collisions
1174
1175
1176 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1177 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1178 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1179 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1180 d.BodySetLinearVel(Body, 0, 0, 0);
1181 d.BodyAddForce(Body, 0, 0, fz);
1182 return;
1183
1184 }
1185 else
1186 {
1187
1188 _zeroFlag = false;
1189
1190 // We're flying and colliding with something
1191 fx = ((_target_velocity.X / m_PIDTau) - vel.X) * (PID_D / 6);
1192 fy = ((_target_velocity.Y / m_PIDTau) - vel.Y) * (PID_D / 6);
1193
1194
1195
1196
1197 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1198
1199 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1100 } 1200 }
1101 d.BodyAddForce(Body, 0, 0, buoyancy);
1102 1201
1202 }
1203
1204 fx *= m_mass;
1205 fy *= m_mass;
1206 //fz *= m_mass;
1207
1208 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1209 if (fx != 0 || fy != 0 || fz != 0)
1210 {
1211 //m_taintdisable = true;
1212 //base.RaiseOutOfBounds(Position);
1213 //d.BodySetLinearVel(Body, fx, fy, 0f);
1214 d.BodyAddForce(Body, fx, fy, fz);
1103 } 1215 }
1104 } 1216 }
1105 else 1217 else
1106 { 1218 {
1219 _zeroPosition = d.BodyGetPosition(Body);
1107 return; 1220 return;
1108 } 1221 }
1109 } 1222 }
@@ -1952,5 +2065,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1952 public override void SetMomentum(PhysicsVector momentum) 2065 public override void SetMomentum(PhysicsVector momentum)
1953 { 2066 {
1954 } 2067 }
2068 public override PhysicsVector PIDTarget { set { m_PIDTarget = value; ; } }
2069 public override bool PIDActive { set { m_usePID = value; } }
2070 public override float PIDTau { set { m_PIDTau = (value * 0.6f); } }
1955 } 2071 }
1956} 2072}