public AuroraODEPrim(string name, byte physicsType, PrimitiveBaseShape shape, Vector3 position, Vector3 size, Quaternion rotation, int material, float friction, float restitution, float gravityMultiplier, float density, AuroraODEPhysicsScene parent_scene) { m_vehicle = new AuroraODEDynamics(); // correct for changed timestep PID_D /= (parent_scene.ODE_STEPSIZE*50f); // original ode fps of 50 PID_G /= (parent_scene.ODE_STEPSIZE*50f); body_autodisable_frames = parent_scene.bodyFramesAutoDisable; prim_geom = IntPtr.Zero; _name = name; PhysicsShapeType = physicsType; _size = size; _position = position; fakepos = 0; _orientation = rotation; fakeori = 0; _pbs = shape; _parent_scene = parent_scene; m_targetSpace = IntPtr.Zero; /* m_isphysical = pisPhysical; if (m_isphysical) m_targetSpace = _parent_scene.space; */ m_isphysical = false; m_forceacc = Vector3.Zero; m_angularforceacc = Vector3.Zero; hasOOBoffsetFromMesh = false; _triMeshData = IntPtr.Zero; SetMaterial(material, friction, restitution, gravityMultiplier, density); CalcPrimBodyData(); _parent_scene.AddSimulationChange(() => changeadd()); }
public AuroraODERayCastRequestManager(AuroraODEPhysicsScene pScene) { m_scene = pScene; nearCallback = near; ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); }
/// <summary> /// Dereference the creator scene so that it can be garbage collected if needed. /// </summary> internal void Dispose() { m_scene = null; if (ContactgeomsArray != IntPtr.Zero) Marshal.FreeHGlobal(ContactgeomsArray); }
// end Step private void MoveLinear(float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent) { if (m_linearMotorDirection.LengthSquared() < 0.0001f) { m_linearMotorDirection = Vector3.Zero; m_newVelocity = Vector3.Zero; } else { Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale); m_lastLinearVelocityVector += (addAmount); m_linearMotorDirection *= (1.0f - 1.0f/m_linearMotorDecayTimescale); // convert requested object velocity to world-referenced vector d.Quaternion rot = d.BodyGetQuaternion(Body); Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object //Vector3 oldVelocity = m_newVelocity; m_newVelocity = m_lastLinearVelocityVector*rotq; // apply obj rotation to velocity vector //if (oldVelocity.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON)) // m_newVelocity.Z += dvel_now.Z; // Preserve the accumulated falling velocity } //if (m_newVelocity.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON)) // m_newVelocity.Z += dvel_now.Z; // Preserve the accumulated falling velocity d.Vector3 dpos = d.BodyGetPosition(Body); Vector3 pos = new Vector3(dpos.X, dpos.Y, dpos.Z); if (!(m_lastPositionVector.X == 0 && m_lastPositionVector.Y == 0 && m_lastPositionVector.Z == 0)) { // Only do this if we have a last position m_lastposChange.X = pos.X - m_lastPositionVector.X; m_lastposChange.Y = pos.Y - m_lastPositionVector.Y; m_lastposChange.Z = pos.Z - m_lastPositionVector.Z; } #region Blocking Change if (m_BlockingEndPoint != Vector3.Zero) { bool needUpdateBody = false; if (pos.X >= (m_BlockingEndPoint.X - 1)) { pos.X -= m_lastposChange.X + 1; needUpdateBody = true; } if (pos.Y >= (m_BlockingEndPoint.Y - 1)) { pos.Y -= m_lastposChange.Y + 1; needUpdateBody = true; } if (pos.Z >= (m_BlockingEndPoint.Z - 1)) { pos.Z -= m_lastposChange.Z + 1; needUpdateBody = true; } if (pos.X <= 0) { pos.X += m_lastposChange.X + 1; needUpdateBody = true; } if (pos.Y <= 0) { pos.Y += m_lastposChange.Y + 1; needUpdateBody = true; } if (needUpdateBody) d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); } #endregion #region Terrain checks float terrainHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); if (pos.Z < terrainHeight - 5) { pos.Z = terrainHeight + 2; m_lastPositionVector = pos; //Make sure that we don't have an explosion the next frame with the posChange d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); } else if (pos.Z < terrainHeight) m_newVelocity.Z += 1; #endregion #region Hover // Check if hovering if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) { // We should hover, get the target height if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) { m_VhoverTargetHeight = (float) _pParentScene.GetWaterLevel(pos.X, pos.Y) + m_VhoverHeight; } if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) { m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; } if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) { m_VhoverTargetHeight = m_VhoverHeight; } float tempHoverHeight = m_VhoverTargetHeight; if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0) { // If body is aready heigher, use its height as target height if (pos.Z > tempHoverHeight) tempHoverHeight = pos.Z; } if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) { if ((pos.Z - tempHoverHeight) > .2 || (pos.Z - tempHoverHeight) < -.2) { float h = tempHoverHeight; float groundHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); if (groundHeight >= tempHoverHeight) h = groundHeight; d.BodySetPosition(Body, pos.X, pos.Y, h); } } else { float herr0 = pos.Z - tempHoverHeight; // Replace Vertical speed with correction figure if significant if (herr0 > 0.01f) { m_newVelocity.Z = -((herr0*50.0f)/m_VhoverTimescale); //KF: m_VhoverEfficiency is not yet implemented } else if (herr0 < -0.01f) { m_newVelocity.Z = -((herr0*50f)/m_VhoverTimescale); } else { m_newVelocity.Z = 0f; } } // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped // m_VhoverTimescale = 0f; // time to acheive height // pTimestep is time since last frame,in secs } #endregion #region No X,Y,Z if ((m_flags & (VehicleFlag.NO_X)) != 0) m_newVelocity.X = 0; if ((m_flags & (VehicleFlag.NO_Y)) != 0) m_newVelocity.Y = 0; if ((m_flags & (VehicleFlag.NO_Z)) != 0) m_newVelocity.Z = 0; #endregion #region Deal with tainted forces // KF: So far I have found no good method to combine a script-requested // .Z velocity and gravity. Therefore only 0g will used script-requested // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; Vector3 TaintedForce = new Vector3(); if (m_forcelist.Count != 0) { try { TaintedForce = m_forcelist.Aggregate(TaintedForce, (current, t) => current + (t)); } catch (IndexOutOfRangeException) { TaintedForce = Vector3.Zero; } catch (ArgumentOutOfRangeException) { TaintedForce = Vector3.Zero; } m_forcelist = new List<Vector3>(); } // force to deltaV m_newVelocity += TaintedForce*(pTimestep/Mass); #endregion #region Deflection //Forward is the prefered direction /*Vector3 deflectionamount = m_newVelocity / (m_linearDeflectionTimescale / pTimestep); //deflectionamount *= m_linearDeflectionEfficiency; if (deflectionamount != Vector3.Zero) { } Vector3 deflection = Vector3.One / deflectionamount; m_newVelocity /= deflection;*/ #endregion #region limitations if (m_newVelocity.LengthSquared() > 1e6f) { m_newVelocity /= m_newVelocity.Length(); m_newVelocity *= 1000f; } else if (m_newVelocity.LengthSquared() < 1e-6f) m_newVelocity = Vector3.Zero; #endregion m_lastPositionVector = parent.Position; float grav = -1*Mass*pTimestep; // Apply velocity if (m_newVelocity != Vector3.Zero) { if ((Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED) && !parent.LinkSetIsColliding) { //Force ODE gravity here!!! } else d.BodySetLinearVel(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z + grav); } // apply friction m_lastLinearVelocityVector.X *= (1.0f - 1/m_linearFrictionTimescale.X); m_lastLinearVelocityVector.Y *= (1.0f - 1/m_linearFrictionTimescale.Y); m_lastLinearVelocityVector.Z *= (1.0f - 1/m_linearFrictionTimescale.Z); }
// end MoveLinear() private void MoveAngular(float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent) { d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); d.Quaternion rot = d.BodyGetQuaternion(Body); Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // Vector3 angularVelocity = Vector3.Zero; /*if ((m_flags & VehicleFlag.MOUSELOOK_STEER) == VehicleFlag.MOUSELOOK_STEER) { if (m_userLookAt != Quaternion.Identity) { Quaternion camrot = Quaternion.Subtract (m_userLookAt, rotq); camrot.Normalize (); m_angularMotorVelocity += Vector3.One * camrot; Console.WriteLine (Vector3.One * camrot); } }*/ if (m_angularMotorDirection.LengthSquared() > 1e-6f) { m_angularMotorVelocity.X = (m_angularMotorDirection.X - angularVelocity.X)/(m_angularMotorTimescale); m_angularMotorVelocity.Y = (m_angularMotorDirection.Y - angularVelocity.Y)/(m_angularMotorTimescale); m_angularMotorVelocity.Z = (m_angularMotorDirection.Z - angularVelocity.Z)/(m_angularMotorTimescale); m_angularMotorDirection *= (1.0f - 1.0f/m_angularMotorDecayTimescale); } else { m_angularMotorVelocity = Vector3.Zero; } // end motor section // Vertical attractor section Vector3 vertattr = Vector3.Zero; Vector3 deflection = Vector3.Zero; Vector3 banking = Vector3.Zero; if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero) { float VAservo = 0; if (Type == Vehicle.TYPE_BOAT) { VAservo = 0.2f/(m_verticalAttractionTimescale); VAservo *= (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency); } else { if (parent.LinkSetIsColliding) VAservo = 0.05f/(m_verticalAttractionTimescale); else VAservo = 0.2f/(m_verticalAttractionTimescale); VAservo *= (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency); } // make a vector pointing up Vector3 verterr = Vector3.Zero; verterr.Z = 1.0f; // rotate it to Body Angle verterr = verterr*rotq; // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. if (verterr.Z < 0.0f) { verterr.X = 2.0f - verterr.X; verterr.Y = 2.0f - verterr.Y; } // Error is 0 (no error) to +/- 2 (max error) // scale it by VAservo verterr = verterr*VAservo; //if (frcount == 0) Console.WriteLine("VAerr=" + verterr); // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. vertattr.X = verterr.Y; vertattr.Y = -verterr.X; vertattr.Z = 0f; // scaling appears better usingsquare-law float bounce = 1.0f - (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency); vertattr.X += bounce*angularVelocity.X; vertattr.Y += bounce*angularVelocity.Y; } // else vertical attractor is off #region Deflection //Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well Vector3 PreferredAxisOfMotion = new Vector3((10*(m_angularDeflectionEfficiency/m_angularDeflectionTimescale)), 0, 0); PreferredAxisOfMotion *= Quaternion.Add(rotq, m_referenceFrame); //Multiply it so that it scales linearly //deflection = PreferredAxisOfMotion; //deflection = ((PreferredAxisOfMotion * m_angularDeflectionEfficiency) / (m_angularDeflectionTimescale / pTimestep)); #endregion #region Banking if (m_bankingEfficiency != 0) { Vector3 dir = Vector3.One*rotq; float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1); //Changes which way it banks in and out of turns //Use the square of the efficiency, as it looks much more how SL banking works float effSquared = (m_bankingEfficiency*m_bankingEfficiency); if (m_bankingEfficiency < 0) effSquared *= -1; //Keep the negative! float mix = Math.Abs(m_bankingMix); if (m_angularMotorVelocity.X == 0) { /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f)) { Vector3 axisAngle; float angle; parent.Orientation.GetAxisAngle(out axisAngle, out angle); Vector3 rotatedVel = parent.Velocity * parent.Orientation; if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0)) m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10; else m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10; }*/ } else banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X)*4; if (!parent.LinkSetIsColliding && Math.Abs(m_angularMotorVelocity.X) > mix) //If they are colliding, we probably shouldn't shove the prim around... probably { float angVelZ = m_angularMotorVelocity.X*-1; /*if(angVelZ > mix) angVelZ = mix; else if(angVelZ < -mix) angVelZ = -mix;*/ //This controls how fast and how far the banking occurs Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0); if (bankingRot.X > 3) bankingRot.X = 3; else if (bankingRot.X < -3) bankingRot.X = -3; bankingRot *= rotq; banking += bankingRot; } m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency; } #endregion #region Downward Force Vector3 downForce = Vector3.Zero; double Zchange = m_lastposChange.Z; if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) //if it isn't going up, don't apply the limiting force { if (Zchange < -0.1f) { if (Zchange < -0.3f) Zchange = -0.3f; //Requires idea of 'up', so use reference frame to rotate it //Add to the X, because that will normally tilt the vehicle downward (if its rotated, it'll be rotated by the ref. frame downForce = (new Vector3(0, ((float) Math.Abs(Zchange)*(pTimestep*_pParentScene.PID_P/4)), 0)); downForce *= rotq; } } #endregion // Sum velocities m_lastAngularVelocity = m_angularMotorVelocity + vertattr + deflection + banking + downForce; #region Linear Motor Offset //Offset section if (m_linearMotorOffset != Vector3.Zero) { //Offset of linear velocity doesn't change the linear velocity, // but causes a torque to be applied, for example... // // IIIII >>> IIIII // IIIII >>> IIIII // IIIII >>> IIIII // ^ // | Applying a force at the arrow will cause the object to move forward, but also rotate // // // The torque created is the linear velocity crossed with the offset //Note: we use the motor, otherwise you will just spin around and we divide by 10 since otherwise we go crazy Vector3 torqueFromOffset = (m_linearMotorDirectionLASTSET/m_linearMotorOffset); if (float.IsNaN(torqueFromOffset.X)) torqueFromOffset.X = 0; if (float.IsNaN(torqueFromOffset.Y)) torqueFromOffset.Y = 0; if (float.IsNaN(torqueFromOffset.Z)) torqueFromOffset.Z = 0; d.BodyAddTorque(Body, torqueFromOffset.X, torqueFromOffset.Y, torqueFromOffset.Z); } #endregion /*if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) { m_lastAngularVelocity.X = 0; m_lastAngularVelocity.Y = 0; }*/ // apply friction // Apply to the body if (m_lastAngularVelocity.LengthSquared() < 0.0001f) { m_lastAngularVelocity = Vector3.Zero; d.BodySetAngularVel(Body, 0, 0, 0); m_angularZeroFlag = true; } else { if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); m_angularZeroFlag = false; m_lastAngularVelocity.X *= (1.0f - 1.0f/m_angularFrictionTimescale.X); m_lastAngularVelocity.Y *= (1.0f - 1.0f/m_angularFrictionTimescale.Y); m_lastAngularVelocity.Z *= (1.0f - 1.0f/m_angularFrictionTimescale.Z); } }
internal void Step(IntPtr pBody, float pTimestep, AuroraODEPhysicsScene pParentScene, AuroraODEPrim parent) { m_body = pBody; if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) return; if (Mass == 0) GetMass(pBody); if (Mass == 0) return; //No noMass vehicles... if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); frcount++; // used to limit debug comment output if (frcount > 100) frcount = 0; // scale time so parameters work as before // until we scale then acording to ode step time MoveLinear(pTimestep, pParentScene, parent); MoveAngular(pTimestep, pParentScene, parent); LimitRotation(pTimestep); }
//end SetDefaultsForType internal void Enable(IntPtr pBody, AuroraODEPrim parent, AuroraODEPhysicsScene pParentScene) { if (m_enabled) return; if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) return; m_body = pBody; d.BodySetGravityMode(Body, true); m_enabled = true; m_lastLinearVelocityVector = parent.Velocity; m_lastPositionVector = parent.Position; m_lastAngularVelocity = parent.RotationalVelocity; parent.ThrottleUpdates = false; GetMass(pBody); }
public ODESpecificAvatar(String avName, AuroraODEPhysicsScene parent_scene, Vector3 pos, Quaternion rotation, Vector3 size) : base(avName, parent_scene, pos, rotation, size) { base._parent_ref = this; }