// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp
//public class Hull m_optionalHull;
public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
{
Vector3 supVec = new Vector3();
float maxDot = -1e30f;
float lenSqr = vec.LengthSquared();
if (lenSqr < 0.0001f)
{
vec = new Vector3(1, 0, 0);
}
else
{
float rlen = 1f / (float)Math.Sqrt(lenSqr);
vec *= rlen;
}
Vector3 vtx;
float newDot;
for (int i = 0; i < VertexCount; i++)
{
GetVertex(i, out vtx);
newDot = Vector3.Dot(vec, vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
public void Vector3_CalculatesLengthSquaredCorrectly()
{
var vectorX = 123.4f;
var vectorY = 567.8f;
var vectorZ = 901.2f;
var vector = new Vector3(vectorX, vectorY, vectorZ);
TheResultingValue(vector.LengthSquared()).WithinDelta(0.1f)
.ShouldBe((vectorX * vectorX) + (vectorY * vectorY) + (vectorZ * vectorZ));
}
public static bool AddPrefab(string prefabName, long positionEntityId, bool wireframe, ulong messageId = 0)
{
if (!MyAPIGateway.Entities.EntityExists(positionEntityId))
return false;
var entity = MyAPIGateway.Entities.GetEntityById(positionEntityId);
var prefab = MyDefinitionManager.Static.GetPrefabDefinition(prefabName);
if (prefab.CubeGrids == null)
{
MyDefinitionManager.Static.ReloadPrefabsFromFile(prefab.PrefabPath);
prefab = MyDefinitionManager.Static.GetPrefabDefinition(prefab.Id.SubtypeName);
}
if (prefab.CubeGrids.Length == 0)
return false;
var worldMatrix = entity.WorldMatrix;
// Use the cubeGrid BoundingBox to determine distance to place.
Vector3I min = Vector3I.MaxValue;
Vector3I max = Vector3I.MinValue;
foreach (var b in prefab.CubeGrids[0].CubeBlocks)
{
min = Vector3I.Min(b.Min, min);
max = Vector3I.Max(b.Min, max);
}
var size = new Vector3(max - min);
var distance = (Math.Sqrt(size.LengthSquared()) * prefab.CubeGrids[0].GridSizeEnum.ToGridLength() / 2) + 2;
var position = worldMatrix.Translation + worldMatrix.Forward * distance; // offset the position out in front of player by 2m.
var offset = position - prefab.CubeGrids[0].PositionAndOrientation.Value.Position;
var tempList = new List<MyObjectBuilder_EntityBase>();
// We SHOULD NOT make any changes directly to the prefab, we need to make a Value copy using Clone(), and modify that instead.
foreach (var grid in prefab.CubeGrids)
{
var gridBuilder = (MyObjectBuilder_CubeGrid)grid.Clone();
gridBuilder.PositionAndOrientation = new MyPositionAndOrientation(grid.PositionAndOrientation.Value.Position + offset, grid.PositionAndOrientation.Value.Forward, grid.PositionAndOrientation.Value.Up);
if (wireframe)
foreach (var cube in gridBuilder.CubeBlocks)
{
cube.IntegrityPercent = 0.01f;
cube.BuildPercent = 0.01f;
}
tempList.Add(gridBuilder);
}
tempList.CreateAndSyncEntities();
return true;
}
///<summary>
/// Computes the expansion of the minkowski sum due to margins in a given direction.
///</summary>
///<param name="marginA">First margin.</param>
///<param name="marginB">Second margin.</param>
///<param name="direction">Extreme point direction.</param>
///<param name="contribution">Margin contribution to the extreme point.</param>
public static void ExpandMinkowskiSum(float marginA, float marginB, ref Vector3 direction, out Vector3 contribution)
{
float lengthSquared = direction.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
//The contribution to the minkowski sum by the margin is:
//direction * marginA - (-direction) * marginB.
Vector3.Multiply(ref direction, (marginA + marginB) / (float)Math.Sqrt(lengthSquared), out contribution);
}
else
{
contribution = new Vector3();
}
}
public bool AddSupportPoint(ref Vector3 newPoint)
{
int index = (BitsToIndices[this.simplexBits ^ 15] & 7) - 1;
this.y[index] = newPoint;
this.yLengthSq[index] = newPoint.LengthSquared();
for (int i = BitsToIndices[this.simplexBits]; i != 0; i = i >> 3)
{
int num2 = (i & 7) - 1;
Vector3 vector = this.y[num2] - newPoint;
this.edges[num2][index] = vector;
this.edges[index][num2] = Vector3.Negate(vector);
this.edgeLengthSq[index][num2] = this.edgeLengthSq[num2][index] = vector.LengthSquared();
}
this.UpdateDeterminant(index);
return this.UpdateSimplex(index);
}
///<summary>
/// Computes the expansion of the minkowski sum due to margins in a given direction.
///</summary>
///<param name="marginA">First margin.</param>
///<param name="marginB">Second margin.</param>
///<param name="direction">Extreme point direction.</param>
///<param name="toExpandA">Margin contribution to the shapeA.</param>
///<param name="toExpandB">Margin contribution to the shapeB.</param>
public static void ExpandMinkowskiSum(float marginA, float marginB, Vector3 direction, ref Vector3 toExpandA, ref Vector3 toExpandB)
{
float lengthSquared = direction.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
lengthSquared = 1 / (float)Math.Sqrt(lengthSquared);
//The contribution to the minkowski sum by the margin is:
//direction * marginA - (-direction) * marginB.
Vector3 contribution;
Vector3.Multiply(ref direction, marginA * lengthSquared, out contribution);
Vector3.Add(ref toExpandA, ref contribution, out toExpandA);
Vector3.Multiply(ref direction, marginB * lengthSquared, out contribution);
Vector3.Subtract(ref toExpandB, ref contribution, out toExpandB);
}
//If the direction is too small, then the expansion values are left unchanged.
}
public bool AddSupportPoint(ref Vector3 newPoint)
{
var index = (BitsToIndices[simplexBits ^ 15] & 7) - 1;
y[index] = newPoint;
yLengthSq[index] = newPoint.LengthSquared();
for (var i = BitsToIndices[simplexBits]; i != 0; i = i >> 3)
{
var num2 = (i & 7) - 1;
var vector = y[num2] - newPoint;
edges[num2][index] = vector;
edges[index][num2] = -(vector);
edgeLengthSq[index][num2] = edgeLengthSq[num2][index] = vector.LengthSquared();
}
UpdateDeterminant(index);
return UpdateSimplex(index);
}
public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity)
{
linearVelocity = (transformB.Translation - transformA.Translation) / timeStep;
Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);
Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat);
Vector3 axis;
float angle = 2 * (float)Math.Acos(dorn.W);
axis = new Vector3(dorn.X, dorn.Y, dorn.Z);
//axis[3] = 0.f;
//check for axis length
float len = axis.LengthSquared();
if (len < MathHelper.Epsilon * MathHelper.Epsilon)
axis = new Vector3(1f, 0f, 0f);
else
axis /= (float)Math.Sqrt(len);
angularVelocity = axis * angle / timeStep;
}
public void AlignClipboardToGravity(Vector3 gravity)
{
if (PreviewGrids.Count > 0 && gravity.LengthSquared() > 0.0001f)
{
gravity.Normalize();
//Vector3 gridLeft = PreviewGrids[0].WorldMatrix.Left;
Vector3 forward = Vector3D.Reject(m_pasteDirForward, gravity);//Vector3.Cross(gravity, gridLeft);
m_pasteDirForward = forward;
m_pasteDirUp = -gravity;
//m_pasteOrientationAngle = 0f;
}
}
internal void MoveAndRotateInternal(Vector3 moveIndicator, Vector2 rotationIndicator, float roll, Vector3 rotationCenter)
{
if (Physics == null)
return;
if (DebugMode)
return;
//Died character
if (Physics.CharacterProxy == null)
{
moveIndicator = Vector3.Zero;
rotationIndicator = Vector2.Zero;
roll = 0;
}
var jetpack = JetpackComp;
bool sprint = moveIndicator.Z != 0 && WantsSprint;
bool walk = WantsWalk;
bool jump = WantsJump;
bool jetpackRunning = jetpack != null && jetpack.Running;
bool canMove = !jetpackRunning && !((m_currentCharacterState == HkCharacterStateType.HK_CHARACTER_IN_AIR || (int)m_currentCharacterState == MyCharacter.HK_CHARACTER_FLYING) && (m_currentJumpTime <= 0)) && (m_currentMovementState != MyCharacterMovementEnum.Died);
bool canRotate = (jetpackRunning || !((m_currentCharacterState == HkCharacterStateType.HK_CHARACTER_IN_AIR || (int)m_currentCharacterState == MyCharacter.HK_CHARACTER_FLYING) && (m_currentJumpTime <= 0))) && (m_currentMovementState != MyCharacterMovementEnum.Died);
float acceleration = 0;
float lastSpeed = m_currentSpeed;
if (jetpackRunning)
{
jetpack.MoveAndRotate(ref moveIndicator, ref rotationIndicator, roll, canRotate);
}
else if (canMove || m_movementsFlagsChanged)
{
if (moveIndicator.LengthSquared() > 0)
moveIndicator = Vector3.Normalize(moveIndicator);
MyCharacterMovementEnum newMovementState = GetNewMovementState(ref moveIndicator, ref rotationIndicator, ref acceleration, sprint, walk, canMove, m_movementsFlagsChanged);
SwitchAnimation(newMovementState);
m_movementsFlagsChanged = false;
SetCurrentMovementState(newMovementState);
if (newMovementState == MyCharacterMovementEnum.Sprinting && StatComp != null)
{
StatComp.ApplyModifier("Sprint");
}
if (!IsIdle)
m_currentWalkDelay = MathHelper.Clamp(m_currentWalkDelay - VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS, 0, m_currentWalkDelay);
if (canMove)
{
float relativeSpeed = 1.0f;
if (MyFakes.ENABLE_CHARACTER_CONTROL_ON_SERVER)
{
relativeSpeed = Sync.IsServer ? 1.0f : Sync.RelativeSimulationRatio;
}
m_currentSpeed = LimitMaxSpeed(m_currentSpeed + (m_currentWalkDelay <= 0 ? acceleration * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS : 0), m_currentMovementState, relativeSpeed);
}
if (Physics.CharacterProxy != null)
{
Physics.CharacterProxy.PosX = m_currentMovementState != MyCharacterMovementEnum.Sprinting ? -moveIndicator.X : 0;
Physics.CharacterProxy.PosY = moveIndicator.Z;
Physics.CharacterProxy.Elevate = 0;
}
if (canMove && m_currentMovementState != MyCharacterMovementEnum.Jump)
{
int sign = Math.Sign(m_currentSpeed);
m_currentSpeed += -sign * m_currentDecceleration * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
if (Math.Sign(sign) != Math.Sign(m_currentSpeed))
m_currentSpeed = 0;
}
if (Physics.CharacterProxy != null)
Physics.CharacterProxy.Speed = m_currentMovementState != MyCharacterMovementEnum.Died ? m_currentSpeed : 0;
if ((jump && m_currentMovementState != MyCharacterMovementEnum.Jump))
{
PlayCharacterAnimation("Jump", MyBlendOption.Immediate, MyFrameOption.StayOnLastFrame, 0.0f, 1.3f);
if (UseNewAnimationSystem)
TriggerCharacterAnimationEvent("jump", true);
if (StatComp != null)
{
StatComp.DoAction("Jump");
StatComp.ApplyModifier("Jump");
}
m_currentJumpTime = JUMP_DURATION;
SetCurrentMovementState(MyCharacterMovementEnum.Jump);
m_canJump = false;
m_frictionBeforeJump = Physics.CharacterProxy.GetHitRigidBody().Friction;
if (Physics.CharacterProxy != null)
{
Physics.CharacterProxy.GetHitRigidBody().ApplyForce(1, WorldMatrix.Up * Definition.JumpForce * MyPerGameSettings.CharacterGravityMultiplier * Physics.Mass);
Physics.CharacterProxy.Jump = true;
}
//VRage.Trace.MyTrace.Send(VRage.Trace.TraceWindow.Default, "jump");
}
if (m_currentJumpTime > 0)
{
m_currentJumpTime -= VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
// If still jumping, allow minor aerial control
if (m_currentJumpTime > 0)
{
Physics.CharacterProxy.GetHitRigidBody().Friction = 0;
Vector3 rotatedVector = WorldMatrix.Forward * -moveIndicator.Z + WorldMatrix.Right * moveIndicator.X;
Physics.CharacterProxy.GetHitRigidBody().ApplyForce(1, rotatedVector * AERIAL_CONTROL_FORCE_MULTIPLIER * Physics.Mass);
}
// If still falling, check if finished.
else
{
MyCharacterMovementEnum afterJumpState = MyCharacterMovementEnum.Standing;
// Restore friction setting upon end-of-jump time.
Physics.CharacterProxy.GetHitRigidBody().Friction = m_frictionBeforeJump;
// If started falling in physics, set the char to correct state.
if (Physics.CharacterProxy != null && (Physics.CharacterProxy.GetState() == HkCharacterStateType.HK_CHARACTER_IN_AIR || (int)Physics.CharacterProxy.GetState() == MyCharacter.HK_CHARACTER_FLYING))
{
StartFalling();
}
// Didn't have time to start falling. Ex. landed on a mountain before started falling.
else
{
if ((moveIndicator.X != 0 || moveIndicator.Z != 0))
{
if (!WantsCrouch)
{
if (moveIndicator.Z < 0)
{
if (sprint)
{
afterJumpState = MyCharacterMovementEnum.Sprinting;
PlayCharacterAnimation("Sprint", MyBlendOption.WaitForPreviousEnd, MyFrameOption.Loop, 0.2f);
}
else
{
afterJumpState = MyCharacterMovementEnum.Walking;
PlayCharacterAnimation("Walk", MyBlendOption.WaitForPreviousEnd, MyFrameOption.Loop, 0.5f);
}
}
else
{
afterJumpState = MyCharacterMovementEnum.BackWalking;
PlayCharacterAnimation("WalkBack", MyBlendOption.WaitForPreviousEnd, MyFrameOption.Loop, 0.5f);
}
}
else
{
if (moveIndicator.Z < 0)
{
afterJumpState = MyCharacterMovementEnum.CrouchWalking;
PlayCharacterAnimation("CrouchWalk", MyBlendOption.WaitForPreviousEnd, MyFrameOption.Loop, 0.2f);
}
else
{
afterJumpState = MyCharacterMovementEnum.CrouchBackWalking;
PlayCharacterAnimation("CrouchWalkBack", MyBlendOption.WaitForPreviousEnd, MyFrameOption.Loop, 0.2f);
}
}
}
else
{
afterJumpState = MyCharacterMovementEnum.Standing;
PlayCharacterAnimation("Idle", MyBlendOption.WaitForPreviousEnd, MyFrameOption.Loop, 0.2f);
}
SoundComp.PlayFallSound();
m_canJump = true;
SetCurrentMovementState(afterJumpState);
}
m_currentJumpTime = 0;
}
}
}
else if (Physics.CharacterProxy != null)
{
Physics.CharacterProxy.Elevate = 0;
}
if (!jetpackRunning)
{
float ratio = 1.0f;
if (MyFakes.ENABLE_CHARACTER_CONTROL_ON_SERVER)
{
ratio = (float)Math.Min(1.0,Sync.RelativeSimulationRatio);
}
if (rotationIndicator.Y != 0 && (canRotate || m_isFalling || m_currentJumpTime > 0))
{
MatrixD rotationMatrix = MatrixD.CreateRotationY((-rotationIndicator.Y * RotationSpeed * CHARACTER_Y_ROTATION_FACTOR * ratio));
MatrixD characterMatrix = MatrixD.CreateWorld(Physics.CharacterProxy.Position, Physics.CharacterProxy.Forward, Physics.CharacterProxy.Up);
Vector3D headBoneTranslation = Vector3D.Zero;
characterMatrix = rotationMatrix * characterMatrix;
Physics.CharacterProxy.Forward = characterMatrix.Forward;
Physics.CharacterProxy.Up = characterMatrix.Up;
}
if (rotationIndicator.X != 0)
{
if (((m_currentMovementState == MyCharacterMovementEnum.Died) && !m_isInFirstPerson)
||
(m_currentMovementState != MyCharacterMovementEnum.Died))
{
SetHeadLocalXAngle(m_headLocalXAngle - rotationIndicator.X * ratio * RotationSpeed);
int headBone = IsInFirstPersonView ? m_headBoneIndex : m_camera3rdBoneIndex;
if (headBone != -1)
{
m_bobQueue.Clear();
m_bobQueue.Enqueue(BoneAbsoluteTransforms[headBone].Translation);
}
}
}
}
if (Physics.CharacterProxy != null)
{
if (Physics.CharacterProxy.LinearVelocity.LengthSquared() > 0.1f)
m_shapeContactPoints.Clear();
}
WantsJump = false;
WantsFlyUp = false;
WantsFlyDown = false;
}
/// <summary>
/// Determines the distance between a point and a plane..
/// </summary>
/// <param name="point">Point to project onto plane.</param>
/// <param name="normal">Normal of the plane.</param>
/// <param name="pointOnPlane">Point located on the plane.</param>
/// <returns>Distance from the point to the plane.</returns>
public static float GetDistancePointToPlane(ref Vector3 point, ref Vector3 normal, ref Vector3 pointOnPlane)
{
Vector3 offset;
Vector3.Subtract(ref point, ref pointOnPlane, out offset);
float dot;
Vector3.Dot(ref normal, ref offset, out dot);
return dot / normal.LengthSquared();
}
void MoveAngular(float pTimestep, ODEPhysicsScene _pParentScene, ODEPrim parent)
{
bool ishovering = false;
d.Vector3 d_angularVelocity = d.BodyGetAngularVel(Body);
d.Vector3 d_lin_vel_now = d.BodyGetLinearVel(Body);
d.Quaternion drotq = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(drotq.X, drotq.Y, drotq.Z, drotq.W);
rotq *= m_referenceFrame; //add reference rotation to rotq
Quaternion irotq = new Quaternion(-rotq.X, -rotq.Y, -rotq.Z, rotq.W);
Vector3 angularVelocity = new Vector3(d_angularVelocity.X, d_angularVelocity.Y, d_angularVelocity.Z);
Vector3 linearVelocity = new Vector3(d_lin_vel_now.X, d_lin_vel_now.Y, d_lin_vel_now.Z);
Vector3 friction = Vector3.Zero;
Vector3 vertattr = Vector3.Zero;
Vector3 deflection = Vector3.Zero;
Vector3 banking = Vector3.Zero;
//limit maximum rotation speed
if(angularVelocity.LengthSquared() > 1e3f) {
angularVelocity = Vector3.Zero;
d.BodySetAngularVel(Body, angularVelocity.X, angularVelocity.Y, angularVelocity.Z);
}
angularVelocity *= irotq; //world to body orientation
if(m_VhoverTimescale * pTimestep <= 300.0f && m_VhoverHeight > 0.0f) ishovering = true;
#region Angular motor
Vector3 motorDirection = Vector3.Zero;
if(Type == Vehicle.TYPE_BOAT) {
//keep z flat for boats, no sidediving lol
Vector3 tmp = new Vector3(0.0f, 0.0f, m_angularMotorDirection.Z);
m_angularMotorDirection.Z = 0.0f;
m_angularMotorDirection += tmp * irotq;
}
if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering){
motorDirection = m_angularMotorDirection * 0.34f; //0.3f;
}
m_angularMotorVelocity.X = (motorDirection.X - angularVelocity.X) / m_angularMotorTimescale;
m_angularMotorVelocity.Y = (motorDirection.Y - angularVelocity.Y) / m_angularMotorTimescale;
m_angularMotorVelocity.Z = (motorDirection.Z - angularVelocity.Z) / m_angularMotorTimescale;
m_angularMotorDirection *= (1.0f - 1.0f/m_angularMotorDecayTimescale);
if(m_angularMotorDirection.LengthSquared() > 0.0f)
{
if(angularVelocity.X > m_angularMotorDirection.X && m_angularMotorDirection.X >= 0.0f) m_angularMotorVelocity.X = 0.0f;
if(angularVelocity.Y > m_angularMotorDirection.Y && m_angularMotorDirection.Y >= 0.0f) m_angularMotorVelocity.Y = 0.0f;
if(angularVelocity.Z > m_angularMotorDirection.Z && m_angularMotorDirection.Z >= 0.0f) m_angularMotorVelocity.Z = 0.0f;
if(angularVelocity.X < m_angularMotorDirection.X && m_angularMotorDirection.X <= 0.0f) m_angularMotorVelocity.X = 0.0f;
if(angularVelocity.Y < m_angularMotorDirection.Y && m_angularMotorDirection.Y <= 0.0f) m_angularMotorVelocity.Y = 0.0f;
if(angularVelocity.Z < m_angularMotorDirection.Z && m_angularMotorDirection.Z <= 0.0f) m_angularMotorVelocity.Z = 0.0f;
}
#endregion
#region friction
float initialFriction = 0.0001f;
if(angularVelocity.X > initialFriction) friction.X += initialFriction;
if(angularVelocity.Y > initialFriction) friction.Y += initialFriction;
if(angularVelocity.Z > initialFriction) friction.Z += initialFriction;
if(angularVelocity.X < -initialFriction) friction.X -= initialFriction;
if(angularVelocity.Y < -initialFriction) friction.Y -= initialFriction;
if(angularVelocity.Z < -initialFriction) friction.Z -= initialFriction;
if(angularVelocity.X > 0.0f)
friction.X += angularVelocity.X * angularVelocity.X / m_angularFrictionTimescale.X;
else
friction.X -= angularVelocity.X * angularVelocity.X / m_angularFrictionTimescale.X;
if(angularVelocity.Y > 0.0f)
friction.Y += angularVelocity.Y * angularVelocity.Y / m_angularFrictionTimescale.Y;
else
friction.Y -= angularVelocity.Y * angularVelocity.Y / m_angularFrictionTimescale.Y;
if(angularVelocity.Z > 0.0f)
friction.Z += angularVelocity.Z * angularVelocity.Z / m_angularFrictionTimescale.Z;
else
friction.Z -= angularVelocity.Z * angularVelocity.Z / m_angularFrictionTimescale.Z;
if(Math.Abs(m_angularMotorDirection.X) > 0.01f) friction.X = 0.0f;
if(Math.Abs(m_angularMotorDirection.Y) > 0.01f) friction.Y = 0.0f;
if(Math.Abs(m_angularMotorDirection.Z) > 0.01f) friction.Z = 0.0f;
#endregion
#region Vertical attraction
if(m_verticalAttractionTimescale < 300)
{
float VAservo = 38.0f / m_verticalAttractionTimescale;
if(Type == Vehicle.TYPE_CAR) VAservo = 10.0f / m_verticalAttractionTimescale;
Vector3 verterr = new Vector3(0.0f, 0.0f, 1.0f);
verterr *= rotq;
vertattr.X = verterr.Y;
vertattr.Y = -verterr.X;
vertattr.Z = 0.0f;
vertattr *= irotq;
//when upsidedown prefer x rotation of body, to keep forward movement direction the same
if(verterr.Z < 0.0f) {
vertattr.Y = -vertattr.Y * 2.0f;
if(vertattr.X < 0.0f) vertattr.X = -2.0f - vertattr.X;
else vertattr.X = 2.0f - vertattr.X;
}
vertattr *= VAservo;
vertattr.X += (vertattr.X - angularVelocity.X) * (0.004f * m_verticalAttractionEfficiency + 0.0001f);
vertattr.Y += (vertattr.Y - angularVelocity.Y) * (0.004f * m_verticalAttractionEfficiency + 0.0001f);
if((m_flags & (VehicleFlag.LIMIT_ROLL_ONLY)) != 0) vertattr.Y = 0.0f;
}
#endregion
#region deflection
//rotates body to direction of movement (linearMovement vector)
/* temporary disabled due to instabilities, needs to be rewritten
if(m_angularDeflectionTimescale < 300)
{
float Dservo = 0.05f * m_angularDeflectionTimescale * m_angularDeflectionEfficiency;
float mag = (float)linearVelocity.LengthSquared();
if(mag > 0.01f) {
linearVelocity.Y = -linearVelocity.Y;
linearVelocity *= rotq;
mag = (float)Math.Sqrt(mag);
linearVelocity.Y /= mag;
linearVelocity.Z /= mag;
deflection.Y = -linearVelocity.Z;
deflection.Z = -linearVelocity.Y;
deflection *= Dservo;
}
}
*/
#endregion
#region banking
if(m_verticalAttractionTimescale < 300 && m_bankingEfficiency > 0) { //vertical attraction must be enabled
float mag = (linearVelocity.X * linearVelocity.X + linearVelocity.Y * linearVelocity.Y);
if(mag > 0.01f) {
mag = (float)Math.Sqrt(mag);
if(mag > 20.0f) mag = 1.0f;
else mag /= 20.0f;
}
else mag = 0.0f;
float b_static = -m_angularMotorDirection.X * 0.12f * (1.0f - m_bankingMix);
float b_dynamic = -m_angularMotorDirection.X * 0.12f * mag * m_bankingMix;
banking.Z = (b_static + b_dynamic - d_angularVelocity.Z) / m_bankingTimescale * m_bankingEfficiency;
}
#endregion
m_lastAngularVelocity = angularVelocity;
if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering) {
angularVelocity += deflection;
angularVelocity -= friction;
}
else {
banking = Vector3.Zero;
}
angularVelocity += m_angularMotorVelocity;
angularVelocity += vertattr;
angularVelocity *= rotq;
angularVelocity += banking;
if(angularVelocity.LengthSquared() < 1e-5f) {
d.BodySetAngularVel(Body, 0, 0, 0);
m_angularZeroFlag = true;
}
else {
d.BodySetAngularVel(Body, angularVelocity.X, angularVelocity.Y, angularVelocity.Z);
m_angularZeroFlag = false;
}
}
public Vector3 GetAngularVelocity(Vector3 control)
{
/*if (m_grid.GridControllers.IsControlledByLocalPlayer || (!m_grid.GridControllers.IsControlledByAnyPlayer && Sync.IsServer) || (false && Sync.IsServer))
{*/
// Not checking whether engines are running, since ControlTorque should be 0 when
// engines are stopped (set by cockpit).
if (ResourceSink.SuppliedRatio > 0f && m_grid.Physics != null && m_grid.Physics.Enabled && !m_grid.Physics.RigidBody.IsFixed)
{
Matrix invWorldRot = m_grid.PositionComp.WorldMatrixInvScaled.GetOrientation();
Matrix worldRot = m_grid.WorldMatrix.GetOrientation();
Vector3 localAngularVelocity = Vector3.Transform(m_grid.Physics.AngularVelocity, ref invWorldRot);
// CH: CAUTION: Don't try to use InertiaTensor, although it might be more intuitive in some cases.
// I tried it and it's not an inverse of the InverseInertiaTensor! Only the InverseInertiaTensor seems to be correct!
var invTensor = m_grid.Physics.RigidBody.InverseInertiaTensor;
Vector3 invTensorVector = new Vector3(invTensor.M11, invTensor.M22, invTensor.M33);
var minInvTensor = invTensorVector.Min();
// Max rotation limiter
float divider = Math.Max(1, minInvTensor * INV_TENSOR_MAX_LIMIT);
// Calculate the velocity correction torque
Vector3 correctionTorque = Vector3.Zero;
Vector3 desiredAcceleration = desiredAcceleration = (m_overrideTargetVelocity - localAngularVelocity) * MyEngineConstants.UPDATE_STEPS_PER_SECOND;
// The correction is done by overridden gyros and by the remaining power of the controlled gyros
// This is not entirely physically correct, but it feels good
float correctionForce = m_maxOverrideForce + m_maxGyroForce * (1.0f - control.Length());
// This is to ensure that the correction is done uniformly in all axes
desiredAcceleration = desiredAcceleration * Vector3.Normalize(invTensorVector);
Vector3 desiredTorque = desiredAcceleration / invTensorVector;
float framesToDesiredVelocity = desiredTorque.Length() / correctionForce;
// If we are very close to the target velocity, just set it without applying the torque
const float minimalBypassVelocity = 0.005f * 0.005f;
if (framesToDesiredVelocity < 0.5f && m_overrideTargetVelocity.LengthSquared() < minimalBypassVelocity)
{
return m_overrideTargetVelocity;
}
if (!Vector3.IsZero(desiredAcceleration, 0.0001f))
{
// The smoothing coefficient is here to avoid the slowdown stopping the ship abruptly, which doesn't look good
float smoothingCoeff = 1.0f - 0.8f / (float)Math.Exp(0.5f * framesToDesiredVelocity);
correctionTorque = Vector3.ClampToSphere(desiredTorque, correctionForce) * 0.95f * smoothingCoeff + desiredTorque * 0.05f * (1.0f - smoothingCoeff);
// A little black magic to make slowdown on large ships bigger
if (m_grid.GridSizeEnum == MyCubeSize.Large)
correctionTorque *= 2.0f;
}
Torque = (control * m_maxGyroForce + correctionTorque) / divider;
Torque *= ResourceSink.SuppliedRatio;
if (Torque.LengthSquared() > 0.0001f)
{
// Manually apply torque and use minimal component of inverted inertia tensor to make rotate same in all axes
var delta = Torque * new Vector3(minInvTensor) * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
var newAngularVelocity = localAngularVelocity + delta;
return Vector3.Transform(newAngularVelocity, ref worldRot);
}
const float stoppingVelocitySq = 0.0003f * 0.0003f;
if (control == Vector3.Zero && m_overrideTargetVelocity == Vector3.Zero && m_grid.Physics.AngularVelocity != Vector3.Zero && m_grid.Physics.AngularVelocity.LengthSquared() < stoppingVelocitySq && m_grid.Physics.RigidBody.IsActive)
{
return Vector3.Zero;
}
}
//}
if (m_grid.Physics != null)
{
return m_grid.Physics.AngularVelocity;
}
else
{
return Vector3.Zero;
}
}
/// <summary>
/// Calculates soft coeficient at target point
/// </summary>
private static float CalculateSoften(float softAreaPlanar, float softAreaVertical, ref Vector3 normal, Vector3 contactToTarget)
{
float planeDist = Math.Abs(Vector3.Dot(normal, contactToTarget));
float flatDist = (float)Math.Sqrt(Math.Max(0, contactToTarget.LengthSquared() - planeDist * planeDist));
float vertSoft = Math.Max(0, 1 - planeDist / softAreaVertical);
float flatSoft = Math.Max(0, 1 - flatDist / softAreaPlanar);
return vertSoft * flatSoft;
}
/// <summary>
/// Updates the movement basis of the horizontal motion constraint.
/// Should be updated automatically by the character on each time step; other code should not need to call this.
/// </summary>
/// <param name="forward">Forward facing direction of the character.</param>
public void UpdateMovementBasis(ref Vector3 forward)
{
Vector3 down = characterBody.orientationMatrix.Down;
horizontalForwardDirection = forward - down * Vector3.Dot(down, forward);
float forwardLengthSquared = horizontalForwardDirection.LengthSquared();
if (forwardLengthSquared < Toolbox.Epsilon)
{
//Use an arbitrary direction to complete the basis.
horizontalForwardDirection = characterBody.orientationMatrix.Forward;
strafeDirection = characterBody.orientationMatrix.Right;
}
else
{
Vector3.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection);
Vector3.Cross(ref down, ref horizontalForwardDirection, out strafeDirection);
//Don't need to normalize the strafe direction; it's the cross product of two normalized perpendicular vectors.
}
Vector3.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d);
Vector3 strafeComponent;
Vector3.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent);
Vector3.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d);
}
///<summary>
/// Gets the extreme point of the shape in local space in a given direction with margin expansion.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public void GetLocalExtremePoint(Vector3 direction, out Vector3 extremePoint)
{
GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
float directionLength = direction.LengthSquared();
if (directionLength > Toolbox.Epsilon)
{
Vector3.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction);
Vector3.Add(ref extremePoint, ref direction, out extremePoint);
}
}
// Get normal from lookup table or calc it
void GetVoxelNormal(MyTemporaryVoxel temporaryVoxel, ref Vector3I coord, ref Vector3I voxelCoord, MyTemporaryVoxel centerVoxel)
{
if (temporaryVoxel.Normal_CalcCounter != m_temporaryVoxelsCounter)
{
Vector3I sampleMin = coord - 1;
Vector3I sampleMax = coord + 1;
var cache = m_cache;
var clampMax = cache.Size3D - 1;
Vector3I.Max(ref sampleMin, ref Vector3I.Zero, out sampleMin);
Vector3I.Min(ref sampleMax, ref clampMax, out sampleMax);
Vector3 normal = new Vector3(
(cache.Content(sampleMin.X, coord.Y, coord.Z) - cache.Content(sampleMax.X, coord.Y, coord.Z)) / MyVoxelConstants.VOXEL_CONTENT_FULL_FLOAT,
(cache.Content(coord.X, sampleMin.Y, coord.Z) - cache.Content(coord.X, sampleMax.Y, coord.Z)) / MyVoxelConstants.VOXEL_CONTENT_FULL_FLOAT,
(cache.Content(coord.X, coord.Y, sampleMin.Z) - cache.Content(coord.X, coord.Y, sampleMax.Z)) / MyVoxelConstants.VOXEL_CONTENT_FULL_FLOAT);
if (normal.LengthSquared() <= 0.000001f)
{
// If voxels surounding voxel for which we want to get normal vector are of the same value, their subtracting leads to zero vector and that can't be used. So following line is hack.
temporaryVoxel.Normal = centerVoxel.Normal;
}
else
{
MyUtils.Normalize(ref normal, out temporaryVoxel.Normal);
}
temporaryVoxel.Normal_CalcCounter = m_temporaryVoxelsCounter;
}
}
public override bool Invoke(ulong steamId, long playerId, string messageText)
{
var match = Regex.Match(messageText, @"/pasteprefab\s{1,}(?<Key>.+)", RegexOptions.IgnoreCase);
if (match.Success)
{
var prefabName = match.Groups["Key"].Value;
var prefabKvp = MyDefinitionManager.Static.GetPrefabDefinitions().FirstOrDefault(kvp => kvp.Key.Equals(prefabName, StringComparison.InvariantCultureIgnoreCase));
MyPrefabDefinition prefab = null;
if (prefabKvp.Value != null)
{
prefab = prefabKvp.Value;
}
int index;
if (prefabName.Substring(0, 1) == "#" && Int32.TryParse(prefabName.Substring(1), out index) && index > 0 && index <= CommandListPrefabs.PrefabCache.Count)
{
prefab = CommandListPrefabs.PrefabCache[index - 1];
}
if (prefab != null)
{
if (prefab.CubeGrids == null)
{
MyDefinitionManager.Static.ReloadPrefabsFromFile(prefab.PrefabPath);
prefab = MyDefinitionManager.Static.GetPrefabDefinition(prefab.Id.SubtypeName);
}
if (prefab.CubeGrids.Count() == 0)
return true;
var worldMatrix = MyAPIGateway.Session.Player.Controller.ControlledEntity.Entity.WorldMatrix;
// Use the cubeGrid BoundingBox to determine distance to place.
Vector3I min = Vector3I.MaxValue;
Vector3I max = Vector3I.MinValue;
foreach (var b in prefab.CubeGrids[0].CubeBlocks)
{
min = Vector3I.Min(b.Min, min);
max = Vector3I.Max(b.Min, max);
}
var size = new Vector3(max - min);
var distance = (float)(Math.Sqrt(size.LengthSquared()) * prefab.CubeGrids[0].GridSizeEnum.ToGridLength() / 2) + 5;
var position = worldMatrix.Translation;
var offset = position - prefab.CubeGrids[0].PositionAndOrientation.Value.Position;
MyAPIGateway.Entities.RemapObjectBuilderCollection(prefab.CubeGrids);
foreach (var grid in prefab.CubeGrids)
{
grid.PositionAndOrientation = new MyPositionAndOrientation(grid.PositionAndOrientation.Value.Position + offset, grid.PositionAndOrientation.Value.Forward, grid.PositionAndOrientation.Value.Up);
}
// only works in Creative mode, both Single and Server (even with paste disabled).
//MyAPIGateway.CubeBuilder.ActivateShipCreationClipboard(prefab.CubeGrids, worldMatrix.Forward, distance);
return true;
}
}
return false;
}
public void UpdateBeforeSimulationOld()
{
if (m_grid.Physics == null)
return;
if (m_grid.Physics.AngularVelocity == Vector3.Zero && ControlTorque == Vector3.Zero)
return;
//if (m_grid.GridControllers.IsControlledByLocalPlayer || (!m_grid.GridControllers.IsControlledByAnyPlayer && Sync.IsServer) || (false && Sync.IsServer))
{
// Not checking whether engines are running, since ControlTorque should be 0 when
// engines are stopped (set by cockpit).
if (ResourceSink.SuppliedRatio > 0f && m_grid.Physics != null && (m_grid.Physics.Enabled || m_grid.Physics.IsWelded) && !m_grid.Physics.RigidBody.IsFixed)
{
Matrix invWorldRot = m_grid.PositionComp.GetWorldMatrixNormalizedInv().GetOrientation();
Matrix worldRot = m_grid.WorldMatrix.GetOrientation();
Vector3 localAngularVelocity = Vector3.Transform(m_grid.Physics.AngularVelocity, ref invWorldRot);
float slowdown = (1 - MAX_SLOWDOWN) * (1 - ResourceSink.SuppliedRatio) + MAX_SLOWDOWN;
var slowdownAngularAcceleration = -localAngularVelocity;// *MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
var invTensor = m_grid.Physics.RigidBody.InverseInertiaTensor;
invTensor.M44 = 1;
var minInvTensor = Math.Min(Math.Min(invTensor.M11, invTensor.M22), invTensor.M33);
var scale = m_grid.Physics.RigidBody.InertiaTensor.Scale;
var slowdownTorque = slowdownAngularAcceleration;
float torqueSlowdownMultiplier = m_grid.GridSizeEnum == MyCubeSize.Large ? MyFakes.SLOWDOWN_FACTOR_TORQUE_MULTIPLIER_LARGE_SHIP : MyFakes.SLOWDOWN_FACTOR_TORQUE_MULTIPLIER;
Vector3 slowdownClamp = new Vector3(m_maxGyroForce * torqueSlowdownMultiplier);
if (m_grid.Physics.IsWelded)
{
//slowdownTorque = Vector3.TransformNormal(slowdownTorque, Matrix.Invert(m_grid.Physics.WeldInfo.Transform));
//only reliable variant
slowdownTorque = Vector3.TransformNormal(slowdownTorque, m_grid.WorldMatrix);
slowdownTorque = Vector3.TransformNormal(slowdownTorque, Matrix.Invert(m_grid.Physics.RigidBody.GetRigidBodyMatrix()));
}
slowdownTorque *= torqueSlowdownMultiplier;
slowdownTorque /= invTensor.Scale;
slowdownTorque = Vector3.Clamp(slowdownTorque, -slowdownClamp, slowdownClamp) * Vector3.IsZeroVector(ControlTorque);
//MyRenderProxy.DebugDrawText2D(new Vector2(300, 260), m_grid.Physics.RigidBody.InertiaTensor.Scale.ToString(), Color.White, 0.8f);
//MyRenderProxy.DebugDrawText2D(new Vector2(300, 280), invTensor.Scale.ToString(), Color.Orange, 0.8f);
if (slowdownTorque.LengthSquared() > 0.0001f)
{
//if(Sandbox.Game.World.MySession.ControlledEntity.Entity.GetTopMostParent() == m_grid)
// MyRenderProxy.DebugDrawText2D(new Vector2(300,320), (slowdownTorque * slowdown).ToString(), Color.White, 0.8f);
m_grid.Physics.AddForce(MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE, null, null, slowdownTorque * slowdown);
}
// Max rotation limiter
float divider = Math.Max(1, minInvTensor * INV_TENSOR_MAX_LIMIT);
Torque = Vector3.Clamp(ControlTorque, -Vector3.One, Vector3.One) * m_maxGyroForce / divider;
Torque *= ResourceSink.SuppliedRatio;
scale = m_grid.Physics.RigidBody.InertiaTensor.Scale;
scale = Vector3.Abs(scale / scale.AbsMax());
if (Torque.LengthSquared() > 0.0001f)
{
var torque = Torque;
if(m_grid.Physics.IsWelded)
{
torque = Vector3.TransformNormal(torque, m_grid.WorldMatrix);
torque = Vector3.TransformNormal(torque, Matrix.Invert(m_grid.Physics.RigidBody.GetRigidBodyMatrix()));
//torque *= new Vector3(-1, 1, -1);//jn: some weird transformation for welded ship
}
m_grid.Physics.AddForce(MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE, null, null, torque * scale);
//if (Sandbox.Game.World.MySession.ControlledEntity.Entity.GetTopMostParent() == m_grid)
// MyRenderProxy.DebugDrawText2D(new Vector2(300,300), (torque * scale).ToString(), Color.Green, 0.8f);
}
const float stoppingVelocitySq = 0.0003f * 0.0003f;
if (ControlTorque == Vector3.Zero && m_grid.Physics.AngularVelocity != Vector3.Zero && m_grid.Physics.AngularVelocity.LengthSquared() < stoppingVelocitySq && m_grid.Physics.RigidBody.IsActive)
{
m_grid.Physics.AngularVelocity = Vector3.Zero;
}
}
}
}
}// end CalculateMass
public void CalcPrimBodyData()
{
if (prim_geom == IntPtr.Zero)
{
// Ubit let's have a initial basic OOB
primOOBsize.X = _size.X;
primOOBsize.Y = _size.Y;
primOOBsize.Z = _size.Z;
primOOBoffset = Vector3.Zero;
}
else
{
d.AABB AABB;
d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
primOOBsize.X = (AABB.MaxX - AABB.MinX);
primOOBsize.Y = (AABB.MaxY - AABB.MinY);
primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
if (!hasOOBoffsetFromMesh)
{
primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f;
primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f;
primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f;
}
}
// also its own inertia and mass
// keep using basic shape mass for now
CalculatePrimMass();
d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
d.MassTranslate(ref primdMass,
primOOBoffset.X,
primOOBoffset.Y,
primOOBoffset.Z);
primOOBsize *= 0.5f; // let obb size be a corner coords
primOOBradiusSQ = primOOBsize.LengthSquared();
}
public override bool Invoke(string messageText)
{
if (messageText.StartsWith("/addwireframe ", StringComparison.InvariantCultureIgnoreCase))
{
var match = Regex.Match(messageText, @"/addwireframe\s{1,}(?<Key>.+)", RegexOptions.IgnoreCase);
if (match.Success)
{
var prefabName = match.Groups["Key"].Value;
var prefabKvp = MyDefinitionManager.Static.GetPrefabDefinitions().FirstOrDefault(kvp => kvp.Key.Equals(prefabName, StringComparison.InvariantCultureIgnoreCase));
MyPrefabDefinition prefab = null;
if (prefabKvp.Value != null)
{
prefab = prefabKvp.Value;
}
int index;
if (prefabName.Substring(0, 1) == "#" && Int32.TryParse(prefabName.Substring(1), out index) && index > 0 && index <= CommandListPrefabs.PrefabCache.Count)
{
prefab = CommandListPrefabs.PrefabCache[index - 1];
}
if (prefab != null)
{
if (prefab.CubeGrids == null)
{
MyDefinitionManager.Static.ReloadPrefabsFromFile(prefab.PrefabPath);
prefab = MyDefinitionManager.Static.GetPrefabDefinition(prefab.Id.SubtypeName);
}
if (prefab.CubeGrids.Count() == 0)
return true;
var worldMatrix = MyAPIGateway.Session.Player.Controller.ControlledEntity.Entity.WorldMatrix;
// Use the cubeGrid BoundingBox to determine distance to place.
Vector3I min = Vector3I.MaxValue;
Vector3I max = Vector3I.MinValue;
prefab.CubeGrids[0].CubeBlocks.ForEach(b => min = Vector3I.Min(b.Min, min));
prefab.CubeGrids[0].CubeBlocks.ForEach(b => max = Vector3I.Max(b.Min, max));
var size = new Vector3(max - min);
var distance = (Math.Sqrt(size.LengthSquared()) * prefab.CubeGrids[0].GridSizeEnum.ToGridLength() / 2) + 2;
var position = worldMatrix.Translation + worldMatrix.Forward * distance; // offset the position out in front of player by 2m.
var offset = position - prefab.CubeGrids[0].PositionAndOrientation.Value.Position;
var tempList = new List<MyObjectBuilder_EntityBase>();
// We SHOULD NOT make any changes directly to the prefab, we need to make a Value copy using Clone(), and modify that instead.
foreach (var grid in prefab.CubeGrids)
{
var gridBuilder = (MyObjectBuilder_CubeGrid)grid.Clone();
gridBuilder.PositionAndOrientation = new MyPositionAndOrientation(grid.PositionAndOrientation.Value.Position + offset, grid.PositionAndOrientation.Value.Forward, grid.PositionAndOrientation.Value.Up);
foreach (var cube in gridBuilder.CubeBlocks)
{
cube.IntegrityPercent = 0.01f;
cube.BuildPercent = 0.01f;
}
tempList.Add(gridBuilder);
}
tempList.CreateAndSyncEntities();
return true;
}
}
}
return false;
}
///<summary>
/// Gets the extreme point of the shape in local space in a given direction.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint)
{
//Is it the tip of the cone?
float sinThetaSquared = radius * radius / (radius * radius + height * height);
//If d.Y * d.Y / d.LengthSquared >= sinthetaSquared
if (direction.Y > 0 && direction.Y * direction.Y >= direction.LengthSquared() * sinThetaSquared)
{
extremePoint = new Vector3(0, .75f * height, 0);
return;
}
//Is it a bottom edge of the cone?
float horizontalLengthSquared = direction.X * direction.X + direction.Z * direction.Z;
if (horizontalLengthSquared > Toolbox.Epsilon)
{
var radOverSigma = radius / Math.Sqrt(horizontalLengthSquared);
extremePoint = new Vector3((float)(radOverSigma * direction.X), -.25f * height, (float)(radOverSigma * direction.Z));
}
else // It's pointing almost straight down...
extremePoint = new Vector3(0, -.25f * height, 0);
}
// end Step
void MoveLinear(float pTimestep, ODEPhysicsScene _pParentScene, ODEPrim parent)
{
bool ishovering = false;
bool bypass_buoyancy = false;
d.Vector3 dpos = d.BodyGetPosition(Body);
d.Vector3 dvel_now = d.BodyGetLinearVel(Body);
d.Quaternion drotq_now = d.BodyGetQuaternion(Body);
Vector3 pos = new Vector3(dpos.X, dpos.Y, dpos.Z);
Vector3 vel_now = new Vector3(dvel_now.X, dvel_now.Y, dvel_now.Z);
Quaternion rotq = new Quaternion(drotq_now.X, drotq_now.Y, drotq_now.Z, drotq_now.W);
rotq *= m_referenceFrame; //add reference rotation to rotq
Quaternion irotq = new Quaternion(-rotq.X, -rotq.Y, -rotq.Z, rotq.W);
m_newVelocity = Vector3.Zero;
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;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
else if(pos.Z < terrainHeight) {
m_newVelocity.Z += 1;
}
#endregion
#region Hover
Vector3 hovervel = Vector3.Zero;
if(m_VhoverTimescale * pTimestep <= 300.0f && m_VhoverHeight > 0.0f) {
ishovering = true;
if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) {
m_VhoverTargetHeight = (float) _pParentScene.GetWaterLevel(pos.X, pos.Y) + 0.3f + m_VhoverHeight;
}
else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) {
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
}
else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) {
m_VhoverTargetHeight = m_VhoverHeight;
}
else {
float waterlevel = (float)_pParentScene.GetWaterLevel(pos.X, pos.Y) + 0.3f;
float terrainlevel = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
if(waterlevel > terrainlevel) {
m_VhoverTargetHeight = waterlevel + m_VhoverHeight;
}
else {
m_VhoverTargetHeight = terrainlevel + 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;
bypass_buoyancy = true; //emulate sl bug
}
}
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 {
hovervel.Z -= ((dvel_now.Z * 0.1f * m_VhoverEfficiency) + (pos.Z - tempHoverHeight)) / m_VhoverTimescale;
hovervel.Z *= 7.0f * (1.0f + m_VhoverEfficiency);
if(hovervel.Z > 50.0f) hovervel.Z = 50.0f;
if(hovervel.Z < -50.0f) hovervel.Z = -50.0f;
}
}
#endregion
#region limitations
//limit maximum velocity
if(vel_now.LengthSquared() > 1e6f) {
vel_now /= vel_now.Length();
vel_now *= 1000f;
d.BodySetLinearVel(Body, vel_now.X, vel_now.Y, vel_now.Z);
}
//block movement in x and y when low velocity
bool enable_ode_gravity = true;
if(vel_now.LengthSquared() < 0.02f) {
d.BodySetLinearVel(Body, 0.0f, 0.0f, 0.0f);
vel_now = Vector3.Zero;
if(parent.LinkSetIsColliding)
enable_ode_gravity = false;
}
#endregion
#region Linear motors
//cancel directions of linear friction for certain vehicles without having effect on ode gravity
Vector3 vt_vel_now = vel_now;
bool no_grav_calc = false;
if((Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON) && m_VehicleBuoyancy != 1.0f) {
vt_vel_now.Z = 0.0f;
no_grav_calc = true;
}
if(!bypass_buoyancy) {
//apply additional gravity force over ode gravity
if(m_VehicleBuoyancy == 1.0f) enable_ode_gravity = false;
else if(m_VehicleBuoyancy != 0.0f && enable_ode_gravity) {
float grav = _pParentScene.gravityz * parent.GravityMultiplier * -m_VehicleBuoyancy;
m_newVelocity.Z += grav * Mass;
}
}
//set ode gravity
d.BodySetGravityMode(Body, enable_ode_gravity);
//add default linear friction (mimic sl friction as much as possible)
float initialFriction = 0.055f;
float defaultFriction = 180f;
Vector3 friction = Vector3.Zero;
if(parent.LinkSetIsColliding || ishovering) {
if(vt_vel_now.X > 0.0f) friction.X += initialFriction;
if(vt_vel_now.Y > 0.0f) friction.Y += initialFriction;
if(vt_vel_now.Z > 0.0f) friction.Z += initialFriction;
if(vt_vel_now.X < 0.0f) friction.X -= initialFriction;
if(vt_vel_now.Y < 0.0f) friction.Y -= initialFriction;
if(vt_vel_now.Z < 0.0f) friction.Z -= initialFriction;
friction += vt_vel_now / defaultFriction;
friction *= irotq;
}
//world -> body orientation
vel_now *= irotq;
vt_vel_now *= irotq;
//add linear friction
if(vt_vel_now.X > 0.0f)
friction.X += vt_vel_now.X * vt_vel_now.X / m_linearFrictionTimescale.X;
else
friction.X -= vt_vel_now.X * vt_vel_now.X / m_linearFrictionTimescale.X;
if(vt_vel_now.Y > 0.0f)
friction.Y += vt_vel_now.Y * vt_vel_now.Y / m_linearFrictionTimescale.Y;
else
friction.Y -= vt_vel_now.Y * vt_vel_now.Y / m_linearFrictionTimescale.Y;
if(vt_vel_now.Z > 0.0f)
friction.Z += vt_vel_now.Z * vt_vel_now.Z / m_linearFrictionTimescale.Z;
else
friction.Z -= vt_vel_now.Z * vt_vel_now.Z / m_linearFrictionTimescale.Z;
friction /= 1.35f; //1.5f;
//add linear forces
//not the best solution, but it is really close to sl motor velocity, and just works
Vector3 motorVelocity = (m_linearMotorDirection * 3.0f - vel_now) / m_linearMotorTimescale / 5.0f; //2.8f;
Vector3 motorfrictionamp = new Vector3(4.0f, 4.0f, 4.0f);
Vector3 motorfrictionstart = new Vector3(1.0f, 1.0f, 1.0f);
motorVelocity *= motorfrictionstart + motorfrictionamp / (m_linearFrictionTimescale * pTimestep);
float addVel = 0.15f;
if(motorVelocity.LengthSquared() > 0.01f) {
if(motorVelocity.X > 0.0f) motorVelocity.X += addVel;
if(motorVelocity.Y > 0.0f) motorVelocity.Y += addVel;
if(motorVelocity.Z > 0.0f) motorVelocity.Z += addVel;
if(motorVelocity.X < 0.0f) motorVelocity.X -= addVel;
if(motorVelocity.Y < 0.0f) motorVelocity.Y -= addVel;
if(motorVelocity.Z < 0.0f) motorVelocity.Z -= addVel;
}
//free run
if(vel_now.X > m_linearMotorDirection.X && m_linearMotorDirection.X >= 0.0f) motorVelocity.X = 0.0f;
if(vel_now.Y > m_linearMotorDirection.Y && m_linearMotorDirection.Y >= 0.0f) motorVelocity.Y = 0.0f;
if(vel_now.Z > m_linearMotorDirection.Z && m_linearMotorDirection.Z >= 0.0f) motorVelocity.Z = 0.0f;
if(vel_now.X < m_linearMotorDirection.X && m_linearMotorDirection.X <= 0.0f) motorVelocity.X = 0.0f;
if(vel_now.Y < m_linearMotorDirection.Y && m_linearMotorDirection.Y <= 0.0f) motorVelocity.Y = 0.0f;
if(vel_now.Z < m_linearMotorDirection.Z && m_linearMotorDirection.Z <= 0.0f) motorVelocity.Z = 0.0f;
//decay linear motor
m_linearMotorDirection *= (1.0f - 1.0f/m_linearMotorDecayTimescale);
#endregion
#region Deflection
//does only deflect on x axis from world orientation with z axis rotated to body
//it is easier to filter out gravity deflection for vehicles(car) without rotation problems
Quaternion irotq_z = irotq;
irotq_z.X = 0.0f;
irotq_z.Y = 0.0f;
float mag = (float)Math.Sqrt(irotq_z.W * irotq_z.W + irotq_z.Z * irotq_z.Z); //normalize
irotq_z.W /= mag;
irotq_z.Z /= mag;
Vector3 vel_defl = new Vector3(dvel_now.X, dvel_now.Y, dvel_now.Z);
vel_defl *= irotq_z;
if(no_grav_calc) {
vel_defl.Z = 0.0f;
if(!parent.LinkSetIsColliding) vel_defl.Y = 0.0f;
}
Vector3 deflection = vel_defl / m_linearDeflectionTimescale * m_linearDeflectionEfficiency * 100.0f;
float deflectionLengthY = Math.Abs(deflection.Y);
float deflectionLengthX = Math.Abs(deflection.X);
deflection.Z = 0.0f;
if((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) == 0) {
deflection.Z = deflectionLengthX;
deflection.X = -deflection.X;
}
if(vel_defl.X < 0.0f) deflection.X = -deflectionLengthY;
else if(vel_defl.X >= 0.0f) deflection.X = deflectionLengthY;
deflection.Y = -deflection.Y;
irotq_z.W = -irotq_z.W;
deflection *= irotq_z;
#endregion
#region Deal with tainted forces
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>();
}
#endregion
#region Add Forces
//add forces
m_newVelocity -= (friction *= Mass / pTimestep);
m_newVelocity += TaintedForce;
motorVelocity *= Mass / pTimestep;
#endregion
#region No X,Y,Z
if((m_flags & (VehicleFlag.NO_X)) != 0)
m_newVelocity.X = -vel_now.X * Mass / pTimestep;
if((m_flags & (VehicleFlag.NO_Y)) != 0)
m_newVelocity.Y = -vel_now.Y * Mass / pTimestep;
if((m_flags & (VehicleFlag.NO_Z)) != 0)
m_newVelocity.Z = -vel_now.Z * Mass / pTimestep;
#endregion
m_newVelocity *= rotq;
m_newVelocity += (hovervel *= Mass / pTimestep);
if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering) {
m_newVelocity += deflection;
motorVelocity *= rotq;
if((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0 && motorVelocity.Z > 0.0f) motorVelocity.Z = 0.0f;
m_newVelocity += motorVelocity;
}
d.BodyAddForce(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z);
}
private void UpdateGyros(bool networkUpdate = false)
{
SlowdownTorque = Vector3.Zero;
if (m_grid.Physics == null)
return;
if (Vector3.IsZero(m_grid.Physics.AngularVelocity,0.001f) && Vector3.IsZero(ControlTorque,0.001f))
return;
if(Sync.IsServer && networkUpdate == false && m_grid.GridSystems.ControlSystem.IsControlled == true && m_grid.GridSystems.ControlSystem.IsLocallyControlled == false)
{
return;
}
//if (m_grid.GridControllers.IsControlledByLocalPlayer || (!m_grid.GridControllers.IsControlledByAnyPlayer && Sync.IsServer) || (false && Sync.IsServer))
{
// Not checking whether engines are running, since ControlTorque should be 0 when
// engines are stopped (set by cockpit).
if (ResourceSink.SuppliedRatio > 0f && m_grid.Physics != null && (m_grid.Physics.Enabled || m_grid.Physics.IsWelded) && !m_grid.Physics.RigidBody.IsFixed)
{
var invTensor = m_grid.Physics.RigidBody.InverseInertiaTensor;
invTensor.M44 = 1;
if (Sync.IsServer || m_grid.GridSystems.ControlSystem.IsLocallyControlled)
{
Matrix invWorldRot = m_grid.PositionComp.WorldMatrixNormalizedInv.GetOrientation();
Vector3 localAngularVelocity = Vector3.Transform(m_grid.Physics.AngularVelocity, ref invWorldRot);
float slowdown = (1 - MAX_SLOWDOWN) * (1 - ResourceSink.SuppliedRatio) + MAX_SLOWDOWN;
SlowdownTorque = -localAngularVelocity;
float torqueSlowdownMultiplier = m_grid.GridSizeEnum == MyCubeSize.Large ? MyFakes.SLOWDOWN_FACTOR_TORQUE_MULTIPLIER_LARGE_SHIP : MyFakes.SLOWDOWN_FACTOR_TORQUE_MULTIPLIER;
Vector3 slowdownClamp = new Vector3(m_maxGyroForce * torqueSlowdownMultiplier);
if (m_grid.Physics.IsWelded)
{
//slowdownTorque = Vector3.TransformNormal(slowdownTorque, Matrix.Invert(m_grid.GetPhysicsBody().WeldInfo.Transform));
//only reliable variant
SlowdownTorque = Vector3.TransformNormal(SlowdownTorque, m_grid.WorldMatrix);
SlowdownTorque = Vector3.TransformNormal(SlowdownTorque, Matrix.Invert(m_grid.Physics.RigidBody.GetRigidBodyMatrix()));
}
// Only multiply the slowdown by the multiplier if we want to move in a different direction in the given axis
if (!localAngularVelocity.IsValid()) localAngularVelocity = Vector3.Zero;
if (!ControlTorque.IsValid()) ControlTorque = Vector3.Zero;
Vector3 selector = Vector3.One - Vector3.IsZeroVector(Vector3.Sign(localAngularVelocity) - Vector3.Sign(ControlTorque));
SlowdownTorque *= torqueSlowdownMultiplier;
SlowdownTorque /= invTensor.Scale;
SlowdownTorque = Vector3.Clamp(SlowdownTorque, -slowdownClamp, slowdownClamp) * selector;
if (SlowdownTorque.LengthSquared() > 0.0001f)
{
//if(Sandbox.Game.World.MySession.Static.ControlledEntity.Entity.GetTopMostParent() == m_grid)
// MyRenderProxy.DebugDrawText2D(new Vector2(300,320), (slowdownTorque * slowdown).ToString(), Color.White, 0.8f);
m_grid.Physics.AddForce(MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE, null, null, SlowdownTorque * slowdown);
}
}
var minInvTensor = Math.Min(Math.Min(invTensor.M11, invTensor.M22), invTensor.M33);
// Max rotation limiter
float divider = Math.Max(1, minInvTensor * INV_TENSOR_MAX_LIMIT);
Torque = Vector3.Clamp(ControlTorque, -Vector3.One, Vector3.One) * m_maxGyroForce / divider;
Torque *= ResourceSink.SuppliedRatio;
var scale = m_grid.Physics.RigidBody.InertiaTensor.Scale;
scale = Vector3.Abs(scale / scale.AbsMax());
if (Torque.LengthSquared() > 0.0001f)
{
float relativeSpeed = Sync.IsServer ? 1.0f : Sync.RelativeSimulationRatio;
var torque = Torque;
if(m_grid.Physics.IsWelded)
{
torque = Vector3.TransformNormal(torque, m_grid.WorldMatrix);
torque = Vector3.TransformNormal(torque, Matrix.Invert(m_grid.Physics.RigidBody.GetRigidBodyMatrix()));
//torque *= new Vector3(-1, 1, -1);//jn: some weird transformation for welded ship
}
m_grid.Physics.AddForce(MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE, null, null, relativeSpeed*torque * scale);
//if (Sandbox.Game.World.MySession.Static.ControlledEntity.Entity.GetTopMostParent() == m_grid)
// MyRenderProxy.DebugDrawText2D(new Vector2(300,300), (torque * scale).ToString(), Color.Green, 0.8f);
}
const float stoppingVelocitySq = 0.0003f * 0.0003f;
if (ControlTorque == Vector3.Zero && m_grid.Physics.AngularVelocity != Vector3.Zero && m_grid.Physics.AngularVelocity.LengthSquared() < stoppingVelocitySq && m_grid.Physics.RigidBody.IsActive)
{
m_grid.Physics.AngularVelocity = Vector3.Zero;
}
}
}
}
/// <summary>
/// Determines the location of the point when projected onto the plane defined by the normal and a point on the plane.
/// </summary>
/// <param name="point">Point to project onto plane.</param>
/// <param name="normal">Normal of the plane.</param>
/// <param name="pointOnPlane">Point located on the plane.</param>
/// <param name="projectedPoint">Projected location of point onto plane.</param>
public static void GetPointProjectedOnPlane(ref Vector3 point, ref Vector3 normal, ref Vector3 pointOnPlane, out Vector3 projectedPoint)
{
float dot;
Vector3.Dot(ref normal, ref point, out dot);
float dot2;
Vector3.Dot(ref pointOnPlane, ref normal, out dot2);
float t = (dot - dot2) / normal.LengthSquared();
Vector3 multiply;
Vector3.Multiply(ref normal, t, out multiply);
Vector3.Subtract(ref point, ref multiply, out projectedPoint);
}
private void UpdateOverriddenGyros()
{
// Not checking whether engines are running, since ControlTorque should be 0 when
// engines are stopped (set by cockpit).
if (ResourceSink.SuppliedRatio > 0f && m_grid.Physics.Enabled && !m_grid.Physics.RigidBody.IsFixed)
{
Matrix invWorldRot = m_grid.PositionComp.WorldMatrixInvScaled.GetOrientation();
Matrix worldRot = m_grid.WorldMatrix.GetOrientation();
Vector3 localAngularVelocity = Vector3.Transform(m_grid.Physics.AngularVelocity, ref invWorldRot);
Vector3 velocityDiff = m_overrideTargetVelocity - localAngularVelocity;
if (velocityDiff == Vector3.Zero)
return;
UpdateOverrideAccelerationRampFrames(velocityDiff);
// acceleration = m/s * (1/s)
Vector3 desiredAcceleration = velocityDiff * (MyEngineConstants.UPDATE_STEPS_PER_SECOND / m_overrideAccelerationRampFrames.Value);
// CH: CAUTION: Don't try to use InertiaTensor, although it might be more intuitive in some cases.
// I tried it and it's not an inverse of the InverseInertiaTensor! Only the InverseInertiaTensor seems to be correct!
var invTensor = m_grid.Physics.RigidBody.InverseInertiaTensor;
Vector3 invTensorVector = new Vector3(invTensor.M11, invTensor.M22, invTensor.M33);
// Calculate the desired velocity correction torque
Vector3 desiredTorque = desiredAcceleration / invTensorVector;
// Calculate the available force for the correction by arbitrarily sum overridden gyros
// and the remaining force force of the controlled gyros
float correctionForce = m_maxOverrideForce + m_maxGyroForce * (1.0f - ControlTorque.Length());
// Reduce the desired torque to the available force
Vector3 availableTorque = Vector3.ClampToSphere(desiredTorque, correctionForce);
Torque = ControlTorque * m_maxGyroForce + availableTorque;
Torque *= ResourceSink.SuppliedRatio;
const float TORQUE_SQ_LEN_TH = 0.0001f;
if (Torque.LengthSquared() < TORQUE_SQ_LEN_TH)
return;
m_grid.Physics.AddForce(MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE, null, null, Torque*Sync.RelativeSimulationRatio);
}
}
} // end MoveLinear()
// Apply the effect of the angular motor.
private void MoveAngular(float pTimestep)
{
// m_angularMotorDirection // angular velocity requested by LSL motor
// m_angularMotorApply // application frame counter
// m_angularMotorVelocity // current angular motor velocity (ramps up and down)
// m_angularMotorTimescale // motor angular velocity ramp up rate
// m_angularMotorDecayTimescale // motor angular velocity decay rate
// m_angularFrictionTimescale // body angular velocity decay rate
// m_lastAngularVelocity // what was last applied to body
// Get what the body is doing, this includes 'external' influences
Vector3 angularVelocity = Prim.ForceRotationalVelocity;
if (m_angularMotorApply > 0)
{
// Rather than snapping the angular motor velocity from the old value to
// a newly set velocity, this routine steps the value from the previous
// value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
// There are m_angularMotorApply steps.
Vector3 origVel = m_angularMotorVelocity;
Vector3 origDir = m_angularMotorDirection;
// ramp up to new value
// new velocity += error / ( time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},origDir={5},vel={6}",
Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
m_angularMotorApply--;
}
else
{
// No motor recently applied, keep the body velocity
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
if (m_angularMotorVelocity.LengthSquared() < 0.00001)
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.2f / (m_verticalAttractionTimescale / pTimestep);
VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
// get present body rotation
Quaternion rotq = Prim.ForceOrientation;
// 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 amounts. 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.
// Error is 0 (no error) to +/- 2 (max error)
if (verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// scale it by VAservo
verterr = verterr * VAservo;
// 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;
VDetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}",
Prim.LocalID, verterr, bounce, vertattr);
} // 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;
VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
}
if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
}
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
Prim.ForceRotationalVelocity = m_lastAngularVelocity;
VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity);
} //end MoveAngular
// Update frames count to obtain a smooth acceleration ramp for overriden gyros
private void UpdateOverrideAccelerationRampFrames(Vector3 velocityDiff)
{
if (m_overrideAccelerationRampFrames == null)
{
float squaredSpeed = velocityDiff.LengthSquared();
const float MIN_ROTATION_SPEED_SQ_TH = (float)((Math.PI / 2) * (Math.PI / 2));
const int ACCELARION_RAMP_FRAMES = (int)MyEngineConstants.UPDATE_STEPS_PER_SECOND * 2;
if (squaredSpeed > MIN_ROTATION_SPEED_SQ_TH)
{
m_overrideAccelerationRampFrames = ACCELARION_RAMP_FRAMES;
}
else
{
const float K_PROP_ACCEL = (ACCELARION_RAMP_FRAMES - 1) / MIN_ROTATION_SPEED_SQ_TH;
m_overrideAccelerationRampFrames = (int)(squaredSpeed * K_PROP_ACCEL) + 1;
}
}
else if (m_overrideAccelerationRampFrames > 1)
{
m_overrideAccelerationRampFrames--;
}
}
private void ResolveSingleBilateral(RigidBody body1, Vector3 pos1, RigidBody body2, Vector3 pos2, float distance, Vector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0;
return;
}
Vector3 rel_pos1 = pos1 - body1.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - body2.CenterOfMassPosition;
Vector3 vel1 = body1.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = body2.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
Matrix world2A = body1.CenterOfMassTransform;
world2A.Origin = Vector3.Zero;
world2A = Matrix.Transpose(world2A);
Matrix world2B = body2.CenterOfMassTransform;
world2B.Origin = Vector3.Zero;
world2B = Matrix.Transpose(world2B);
Vector3 m_aJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos1, normal), world2A);
Vector3 m_bJ = Vector3.TransformCoordinate(Vector3.Cross(rel_pos2, -normal), world2B);
Vector3 m_0MinvJt = body1.InvInertiaDiagLocal * m_aJ;
Vector3 m_1MinvJt = body2.InvInertiaDiagLocal * m_bJ;
float jacDiagAB = body1.InvMass + Vector3.Dot(m_0MinvJt, m_aJ) + body2.InvMass + Vector3.Dot(m_1MinvJt, m_bJ);
float jacDiagABInv = 1.0f / jacDiagAB;
float rel_vel = Vector3.Dot(normal, vel);
//todo: move this into proper structure
const float contactDamping = 0.2f;
#if ONLY_USE_LINEAR_MASS
float massTerm = 1.0f / (body1.InvMass + body2.InvMass);
impulse = - contactDamping * rel_vel * massTerm;
#else
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
public void TestStaticFn_LengthSquared_i ()
{
Vector3 a = new Vector3(3, -4, 12);
Double expected = 169;
Double result = a.LengthSquared();
Assert.That(result, Is.EqualTo(expected));
}
