コード例 #1
0
		// 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;
		}
コード例 #2
0
 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;
        }
コード例 #4
0
        ///<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();
            }
        }
コード例 #5
0
 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);
 }
コード例 #6
0
 ///<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.
 }
コード例 #7
0
ファイル: Gjk.cs プロジェクト: Gartley/ss13remake
 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);
 }
コード例 #8
0
ファイル: TransformUtil.cs プロジェクト: Belxjander/Asuna
		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;
		}
コード例 #9
0
        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;
            }
        }
コード例 #10
0
        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;
        }
コード例 #11
0
 /// <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();
 }
コード例 #12
0
        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;
            }
        }
コード例 #13
0
        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;
            }
        }
コード例 #14
0
        /// <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;
        }
コード例 #15
0
        /// <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);
        }
コード例 #16
0
        ///<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);
            }
        }
コード例 #17
0
        //  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;
            }
        }
コード例 #18
0
        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;
        }
コード例 #19
0
        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;
                    }
                }
            }
        }
コード例 #20
0
ファイル: AODEPrim.cs プロジェクト: chazzmac/Aurora-Sim
        }// 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;
        }
コード例 #22
0
ファイル: ConeShape.cs プロジェクト: d3x0r/Voxelarium
        ///<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);


        }
コード例 #23
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);
        }
コード例 #24
0
        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;
                    }
                }
            }
        }
コード例 #25
0
 /// <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);
 }
コード例 #26
0
        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);
            }
        }
コード例 #27
0
ファイル: BSDynamics.cs プロジェクト: CCIR/opensim
        } // 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
コード例 #28
0
 // 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--;
     }
 }
コード例 #29
0
        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
        }
コード例 #30
0
        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));
        }