private void MoveAngular(float timestep) { /* * private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor * private int m_angularMotorApply = 0; // application frame counter * private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) * private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate * private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate * private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate * private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body */ // Get what the body is doing, this includes 'external' influences btVector3 angularVelocity = m_body.getInterpolationAngularVelocity(); // Vector3 angularVelocity = Vector3.Zero; if (m_angularMotorApply > 0) { // ramp up to new value // current velocity += error / (time to get there / step interval) // requested speed - last motor speed m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / timestep); m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / timestep); m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / timestep); m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected // velocity may still be acheived. } else { // no motor recently applied, keep the body velocity /* m_angularMotorVelocity.X = angularVelocity.X; * m_angularMotorVelocity.Y = angularVelocity.Y; * m_angularMotorVelocity.Z = angularVelocity.Z; */ // and decay the velocity m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / timestep); } // end motor section // Vertical attractor section Vector3 vertattr = Vector3.Zero; if (m_verticalAttractionTimescale < 300) { float VAservo = 0.2f / (m_verticalAttractionTimescale * timestep); // get present body rotation btQuaternion rot = m_body.getWorldTransform().getRotation(); Quaternion rotq = new Quaternion(rot.getX(), rot.getY(), rot.getZ(), rot.getW()); // 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.getX(); vertattr.Y += bounce * angularVelocity.getY(); } // else vertical attractor is off // m_lastVertAttractor = vertattr; // Bank section tba // Deflection section tba // Sum velocities m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) { m_lastAngularVelocity.X = 0; m_lastAngularVelocity.Y = 0; } // apply friction Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / timestep); m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; // Apply to the body m_body.setAngularVelocity(new btVector3(m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z)); }
private void MoveLinear(float timestep) { if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant { // add drive to body Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / timestep); m_lastLinearVelocityVector += (addAmount * 10); // lastLinearVelocityVector is the current body velocity vector? // This will work temporarily, but we really need to compare speed on an axis // KF: Limit body velocity to applied velocity? if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) { m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; } if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) { m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; } if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) { m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; } // decay applied velocity Vector3 decayfraction = ((Vector3.One / (m_linearMotorDecayTimescale / timestep))); //Console.WriteLine("decay: " + decayfraction); m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f; //Console.WriteLine("actual: " + m_linearMotorDirection); } else { // requested is not significant // if what remains of applied is small, zero it. if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) { m_lastLinearVelocityVector = Vector3.Zero; } } // convert requested object velocity to world-referenced vector m_dir = m_lastLinearVelocityVector; btQuaternion rot = m_body.getWorldTransform().getRotation(); Quaternion rotq = new Quaternion(rot.getX(), rot.getY(), rot.getZ(), rot.getW()); // rotq = rotation of object m_dir *= rotq; // apply obj rotation to velocity vector // add Gravity andBuoyancy // 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. Vector3 grav = Vector3.Zero; // There is some gravity, make a gravity force vector // that is applied after object velocity. float objMass = m_prim.Mass; // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; //Rev: bullet does gravity internally grav.Z = -parent_scene.gravityz * objMass * m_VehicleBuoyancy; //parent_scene.gravityz/* * objMass*/ * (1f - m_VehicleBuoyancy); // Preserve the current Z velocity btVector3 pos = m_body.getWorldTransform().getOrigin(); btVector3 newpos = pos; m_dir.Z = m_prim.Velocity.Z; // Preserve the accumulated falling velocity Vector3 posChange = new Vector3(); posChange.X = newpos.getX() - m_lastPositionVector.getX(); posChange.Y = newpos.getY() - m_lastPositionVector.getY(); posChange.Z = newpos.getZ() - m_lastPositionVector.getZ(); btQuaternion Orientation2 = m_body.getWorldTransform().getRotation(); /*if (m_BlockingEndPoint != Vector3.Zero) * { * if (newpos.getX() >= (m_BlockingEndPoint.X - (float)1)) * newpos.setX(newpos.getX() - (posChange.X + 1)); * if (newpos.getY() >= (m_BlockingEndPoint.Y - (float)1)) * newpos.setY(newpos.getY() - (posChange.Y + 1)); * if (newpos.getZ() >= (m_BlockingEndPoint.Z - (float)1)) * newpos.setZ(newpos.getZ() - (posChange.Z + 1)); * if (newpos.getX() <= 0) * newpos.setX(newpos.getX() + (posChange.X + 1)); * if (newpos.getY() <= 0) * newpos.setY(newpos.getY() + (posChange.Y + 1)); * } */ if (newpos.getZ() < parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY())) { newpos.setZ(parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY()) + 2); } // Check if hovering if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) { float diff = (newpos.getZ() - m_VhoverTargetHeight); // We should hover, get the target height if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0) { m_VhoverTargetHeight = parent_scene.GetWaterLevel() + m_VhoverHeight; } if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) { m_VhoverTargetHeight = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY()) + m_VhoverHeight; } if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) { m_VhoverTargetHeight = m_VhoverHeight; } if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0) { // If body is aready heigher, use its height as target height if (newpos.getZ() > m_VhoverTargetHeight) { m_VhoverTargetHeight = newpos.getZ(); } } if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) { if (diff > .2 || diff < -.2) { newpos.setValue(newpos.getX(), newpos.getY(), m_VhoverTargetHeight); btTransform trans = new btTransform(Orientation2, newpos); m_body.setWorldTransform(trans); } } else { // Replace Vertical speed with correction figure if significant if (Math.Abs(diff) > 0.01f) { m_dir.Z = -((diff * timestep * 50.0f) / m_VhoverTimescale); } else { m_dir.Z = 0f; } } } /*if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) * { * //Start Experimental Values * if (Zchange > .3) * grav.Z = (float)(grav.Z * 3); * if (Zchange > .15) * grav.Z = (float)(grav.Z * 2); * if (Zchange > .75) * grav.Z = (float)(grav.Z * 1.5); * if (Zchange > .05) * grav.Z = (float)(grav.Z * 1.25); * if (Zchange > .025) * grav.Z = (float)(grav.Z * 1.125); * * float terraintemp = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY()); * float postemp = (pos.getZ() - terraintemp); * * if (postemp > 2.5f) * grav.Z = (float)(grav.Z * 1.037125); * //End Experimental Values * }*/ if ((m_flags & (VehicleFlag.NO_X)) != 0) { m_dir.X = 0; } if ((m_flags & (VehicleFlag.NO_Y)) != 0) { m_dir.Y = 0; } if ((m_flags & (VehicleFlag.NO_Z)) != 0) { m_dir.Z = 0; } m_lastPositionVector = new btVector3(m_prim.Position.X, m_prim.Position.Y, m_prim.Position.Z); // Apply velocity //if(m_dir != Vector3.Zero) // m_body.setLinearVelocity(new btVector3(m_dir.X, m_dir.Y, m_dir.Z)); m_body.applyCentralImpulse(new btVector3(m_dir.X, m_dir.Y, m_dir.Z)); // apply gravity force //m_body.applyCentralImpulse(new btVector3(0, 0, 9.8f)); /*ector3 newpos2 = new Vector3(newpos.getX(), newpos.getY(), newpos.getZ()); * if (newpos2.X != m_prim.Position.X || newpos2.Y != m_prim.Position.Y || newpos2.Z != m_prim.Position.Z) * { * btTransform trans = new btTransform(Orientation2, newpos); * m_body.setWorldTransform(trans); * }*/ // apply friction Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / timestep); m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; }
private void LimitRotation(float timestep) { btQuaternion rot = m_body.getWorldTransform().getRotation(); bool changed = false; if (m_RollreferenceFrame != Quaternion.Identity) { if (rot.getX() >= m_RollreferenceFrame.X) { rot = new btQuaternion(rot.getX() - (m_RollreferenceFrame.X / 2), rot.getY(), rot.getZ(), rot.getW()); } if (rot.getX() <= -m_RollreferenceFrame.X) { rot = new btQuaternion(rot.getX() + (m_RollreferenceFrame.X / 2), rot.getY(), rot.getZ(), rot.getW()); } if (rot.getY() >= m_RollreferenceFrame.Y) { rot = new btQuaternion(rot.getX(), rot.getY() - (m_RollreferenceFrame.Y / 2), rot.getZ(), rot.getW()); } if (rot.getY() <= -m_RollreferenceFrame.Y) { rot = new btQuaternion(rot.getX(), rot.getY() + (m_RollreferenceFrame.Y / 2), rot.getZ(), rot.getW()); } changed = true; } if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0) { rot = new btQuaternion(0, 0, rot.getZ(), rot.getW()); changed = true; } if (changed) { btTransform trans = m_body.getWorldTransform(); trans.setRotation(rot); m_body.setWorldTransform(trans); } }