public WebGrid(MyCubeGrid grid) { this.Name = grid.DisplayName; this.BlockCount = grid.BlocksCount; this.Position = new WebVector3D(grid.WorldMatrix.Translation); this.Rotation = new WebQuatD(QuaternionD.CreateFromRotationMatrix(grid.WorldMatrix.GetOrientation())); }
// Transform by QuaternionD public void PlaneTransformTest2() { PlaneD target = new PlaneD(1, 2, 3, 4); target = PlaneD.Normalize(target); Matrix4x4D m = Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) * Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) * Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f)); QuaternionD q = QuaternionD.CreateFromRotationMatrix(m); PlaneD expected = new PlaneD(); double x = target.Normal.X, y = target.Normal.Y, z = target.Normal.Z, w = target.D; expected.Normal = new Vector3D( x * m.M11 + y * m.M21 + z * m.M31 + w * m.M41, x * m.M12 + y * m.M22 + z * m.M32 + w * m.M42, x * m.M13 + y * m.M23 + z * m.M33 + w * m.M43); expected.D = x * m.M14 + y * m.M24 + z * m.M34 + w * m.M44; PlaneD actual; actual = PlaneD.Transform(target, q); Assert.True(MathHelper.Equal(expected, actual), "PlaneD.Transform did not return the expected value."); }
public void Vector3TransformByQuaternionTest() { Vector3D v = new Vector3D(1.0f, 2.0f, 3.0f); Matrix4x4D m = Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0f)) * Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0f)) * Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0f)); QuaternionD q = QuaternionD.CreateFromRotationMatrix(m); Vector3D expected = Vector3D.Transform(v, m); Vector3D actual = Vector3D.Transform(v, q); Assert.True(MathHelper.Equal(expected, actual), "Vector3D.Transform did not return the expected value."); }
public void Vector2TransformByQuaternionTest() { Vector2D v = new Vector2D(1.0, 2.0); Matrix4x4D m = Matrix4x4D.CreateRotationX(MathHelper.ToRadians(30.0)) * Matrix4x4D.CreateRotationY(MathHelper.ToRadians(30.0)) * Matrix4x4D.CreateRotationZ(MathHelper.ToRadians(30.0)); QuaternionD q = QuaternionD.CreateFromRotationMatrix(m); Vector2D expected = Vector2D.Transform(v, m); Vector2D actual = Vector2D.Transform(v, q); Assert.That(actual, Is.EqualTo(expected).Using <Vector2D>((a, b) => a.Equals(b, 1e-15)), "Vector2D.Transform did not return the expected value."); }
public void QuaternionDFromRotationMatrixTest1() { Matrix4x4D matrix = Matrix4x4D.Identity; QuaternionD expected = new QuaternionD(0.0f, 0.0f, 0.0f, 1.0f); QuaternionD actual = QuaternionD.CreateFromRotationMatrix(matrix); Assert.True(MathHelper.Equal(expected, actual), $"QuaternionD.CreateFromRotationMatrix did not return the expected value: expected {expected} actual {actual}"); // make sure convert back to matrix is same as we passed matrix. Matrix4x4D m2 = Matrix4x4D.CreateFromQuaternion(actual); Assert.True(MathHelper.Equal(matrix, m2), $"QuaternionD.CreateFromQuaternionD did not return the expected value: matrix {matrix} m2 {m2}"); }
public void QuaternionDFromRotationMatrixWithScaledMatrixTest3() { double angle = MathHelper.ToRadians(180.0f); Matrix4x4D matrix = Matrix4x4D.CreateRotationX(angle) * Matrix4x4D.CreateRotationY(angle); QuaternionD expected = QuaternionD.CreateFromAxisAngle(Vector3D.UnitY, angle) * QuaternionD.CreateFromAxisAngle(Vector3D.UnitX, angle); QuaternionD actual = QuaternionD.CreateFromRotationMatrix(matrix); Assert.True(MathHelper.EqualRotation(expected, actual), $"QuaternionD.CreateFromRotationMatrix did not return the expected value: expected {expected} actual {actual}"); // make sure convert back to matrix is same as we passed matrix. Matrix4x4D m2 = Matrix4x4D.CreateFromQuaternion(actual); Assert.True(MathHelper.Equal(matrix, m2), $"QuaternionD.CreateFromQuaternionD did not return the expected value: matrix {matrix} m2 {m2}"); }
public void QuaternionDFromRotationMatrixTest4() { for (double angle = 0.0f; angle < 720.0f; angle += 10.0f) { Matrix4x4D matrix = Matrix4x4D.CreateRotationZ(angle); QuaternionD expected = QuaternionD.CreateFromAxisAngle(Vector3D.UnitZ, angle); QuaternionD actual = QuaternionD.CreateFromRotationMatrix(matrix); Assert.True(MathHelper.EqualRotation(expected, actual), $"QuaternionD.CreateFromRotationMatrix angle:{angle} did not return the expected value: expected {expected} actual {actual}"); // make sure convert back to matrix is same as we passed matrix. Matrix4x4D m2 = Matrix4x4D.CreateFromQuaternion(actual); Assert.True(MathHelper.Equal(matrix, m2), $"QuaternionD.CreateFromQuaternionD angle:{angle} did not return the expected value: matrix {matrix} m2 {m2}"); } }
public DetachEffectsPacket(IMySlimBlock block) { Matrix localMatrix; block.Orientation.GetMatrix(out localMatrix); block.ComputeWorldCenter(out Position); MatrixD wm = localMatrix * block.CubeGrid.WorldMatrix; Orientation = QuaternionD.CreateFromRotationMatrix(wm); Velocity = block.CubeGrid.Physics.LinearVelocity; ModelBB = block.FatBlock?.LocalAABB; BlockDefId = block.BlockDefinition.Id; }
public void CutsceneUpdate() { if (!m_nodeActivated) { //new node MySandboxGame.Log.WriteLineAndConsole(m_currentCutscene.Name + ": " + m_currentNodeIndex.ToString()); m_nodeActivated = true; m_nodeStartMatrix = m_currentCameraMatrix; m_nodeEndMatrix = m_currentCameraMatrix; m_nodeStartFOV = m_currentFOV; m_moveTarget = null; m_rotateTarget = null; m_waypoints.Clear(); m_eventDelay = float.MaxValue; if (m_currentNode.Event != null && m_currentNode.Event.Length > 0 && MyVisualScriptLogicProvider.CutsceneNodeEvent != null) { if (m_currentNode.EventDelay <= 0) { MyVisualScriptLogicProvider.CutsceneNodeEvent(m_currentNode.Event); } else { m_eventDelay = m_currentNode.EventDelay; } } //rotation if (m_currentNode.LookAt != null && m_currentNode.LookAt.Length > 0) { MyEntity entity = MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.LookAt); if (entity != null) { m_nodeStartMatrix = MatrixD.CreateLookAtInverse(m_currentCameraMatrix.Translation, m_rotateTarget.PositionComp.GetPosition(), m_currentCameraMatrix.Up); m_nodeEndMatrix = m_nodeStartMatrix; } } if (m_currentNode.SetRorationLike != null && m_currentNode.SetRorationLike.Length > 0) { MyEntity entity = MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.SetRorationLike); if (entity != null) { m_nodeStartMatrix = entity.WorldMatrix; m_nodeEndMatrix = m_nodeStartMatrix; } } if (m_currentNode.RotateLike != null && m_currentNode.RotateLike.Length > 0) { MyEntity entity = MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.RotateLike); if (entity != null) { m_nodeEndMatrix = entity.WorldMatrix; } } if (m_currentNode.RotateTowards != null && m_currentNode.RotateTowards.Length > 0) { m_rotateTarget = m_currentNode.RotateTowards.Length > 0 ? MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.RotateTowards) : null; } if (m_currentNode.LockRotationTo != null) { m_lookTarget = m_currentNode.LockRotationTo.Length > 0 ? MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.LockRotationTo) : null; } //position m_nodeStartMatrix.Translation = m_currentCameraMatrix.Translation; m_nodeEndMatrix.Translation = m_currentCameraMatrix.Translation; if (m_currentNode.SetPositionTo != null && m_currentNode.SetPositionTo.Length > 0) { MyEntity entity = MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.SetPositionTo); if (entity != null) { m_nodeStartMatrix.Translation = entity.WorldMatrix.Translation; m_nodeEndMatrix.Translation = entity.WorldMatrix.Translation; } } if (m_currentNode.AttachTo != null) { if (m_currentNode.AttachTo != null) { m_attachedPositionTo = m_currentNode.AttachTo.Length > 0 ? MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.AttachTo) : null; m_attachedPositionOffset = m_attachedPositionTo != null?Vector3D.Transform(m_currentCameraMatrix.Translation, m_attachedPositionTo.PositionComp.WorldMatrixInvScaled) : Vector3D.Zero; m_attachedRotationTo = m_attachedPositionTo; m_attachedRotationOffset = m_currentCameraMatrix * m_attachedRotationTo.PositionComp.WorldMatrixInvScaled; m_attachedRotationOffset.Translation = Vector3D.Zero; } } else { if (m_currentNode.AttachPositionTo != null) { m_attachedPositionTo = m_currentNode.AttachPositionTo.Length > 0 ? MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.AttachPositionTo) : null; m_attachedPositionOffset = m_attachedPositionTo != null?Vector3D.Transform(m_currentCameraMatrix.Translation, m_attachedPositionTo.PositionComp.WorldMatrixInvScaled) : Vector3D.Zero; } if (m_currentNode.AttachRotationTo != null) { m_attachedRotationTo = m_currentNode.AttachRotationTo.Length > 0 ? MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.AttachRotationTo) : null; m_attachedRotationOffset = m_currentCameraMatrix * m_attachedRotationTo.PositionComp.WorldMatrixInvScaled; m_attachedRotationOffset.Translation = Vector3D.Zero; } } if (m_currentNode.MoveTo != null && m_currentNode.MoveTo.Length > 0) { m_moveTarget = m_currentNode.MoveTo.Length > 0 ? MyVisualScriptLogicProvider.GetEntityByName(m_currentNode.MoveTo) : null; } //waypoints if (m_currentNode.Waypoints != null && m_currentNode.Waypoints.Length > 0) { MyEntity entity; bool first = true; foreach (var waypoint in m_currentNode.Waypoints) { if (waypoint.Name.Length > 0) { entity = MyVisualScriptLogicProvider.GetEntityByName(waypoint.Name); if (entity != null) { m_waypoints.Add(entity.WorldMatrix); if (first) { m_lastUpVector = entity.WorldMatrix.Up; first = false; } } } } if (m_waypoints.Count > 0) { if (m_waypoints.Count < 3) { m_nodeEndMatrix.Translation = m_waypoints[m_waypoints.Count - 1].Translation; m_waypoints.Clear(); } else if (m_waypoints.Count == 2) { m_nodeStartMatrix = m_waypoints[0]; m_nodeEndMatrix = m_waypoints[1]; } } } m_currentCameraMatrix = m_nodeStartMatrix; } //update time m_currentTime += MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS; float timeRatio = m_currentNode.Time > 0 ? MathHelper.Clamp(m_currentTime / m_currentNode.Time, 0f, 1f) : 1f; //event if (m_currentTime >= m_eventDelay) { m_eventDelay = float.MaxValue; MyVisualScriptLogicProvider.CutsceneNodeEvent(m_currentNode.Event); } //end position if (m_moveTarget != null) { m_nodeEndMatrix.Translation = m_moveTarget.PositionComp.GetPosition(); } //update position Vector3D newPos = m_currentCameraMatrix.Translation; if (m_attachedPositionTo != null) { if (!m_attachedPositionTo.Closed) { newPos = Vector3D.Transform(m_attachedPositionOffset, m_attachedPositionTo.PositionComp.WorldMatrix); } } else if (m_waypoints.Count > 2) { double segmentTime = 1f / (m_waypoints.Count - 1); int segment = (int)Math.Floor(timeRatio / segmentTime); if (segment > m_waypoints.Count - 2) { segment = m_waypoints.Count - 2; } double segmentRatio = (timeRatio - segment * segmentTime) / segmentTime; if (segment == 0) { //first path segment newPos = MathHelper.CalculateBezierPoint(segmentRatio, m_waypoints[segment].Translation, m_waypoints[segment].Translation, m_waypoints[segment + 1].Translation - (m_waypoints[segment + 2].Translation - m_waypoints[segment].Translation) / 4, m_waypoints[segment + 1].Translation); } else if (segment >= m_waypoints.Count - 2) { //last path segment newPos = MathHelper.CalculateBezierPoint(segmentRatio, m_waypoints[segment].Translation, m_waypoints[segment].Translation + (m_waypoints[segment + 1].Translation - m_waypoints[segment - 1].Translation) / 4, m_waypoints[segment + 1].Translation, m_waypoints[segment + 1].Translation); } else { //middle path segment newPos = MathHelper.CalculateBezierPoint(segmentRatio, m_waypoints[segment].Translation, m_waypoints[segment].Translation + (m_waypoints[segment + 1].Translation - m_waypoints[segment - 1].Translation) / 4, m_waypoints[segment + 1].Translation - (m_waypoints[segment + 2].Translation - m_waypoints[segment].Translation) / 4, m_waypoints[segment + 1].Translation); } } else if (m_nodeStartMatrix.Translation != m_nodeEndMatrix.Translation) { newPos = new Vector3D( MathHelper.SmoothStep(m_nodeStartMatrix.Translation.X, m_nodeEndMatrix.Translation.X, timeRatio), MathHelper.SmoothStep(m_nodeStartMatrix.Translation.Y, m_nodeEndMatrix.Translation.Y, timeRatio), MathHelper.SmoothStep(m_nodeStartMatrix.Translation.Z, m_nodeEndMatrix.Translation.Z, timeRatio)); } //end rotation if (m_rotateTarget != null) { m_nodeEndMatrix = MatrixD.CreateLookAtInverse(m_currentCameraMatrix.Translation, m_rotateTarget.PositionComp.GetPosition(), m_nodeStartMatrix.Up); } //update rotation if (m_lookTarget != null) { if (!m_lookTarget.Closed) { m_currentCameraMatrix = MatrixD.CreateLookAtInverse(newPos, m_lookTarget.PositionComp.GetPosition(), m_waypoints.Count > 2 ? m_lastUpVector : m_currentCameraMatrix.Up); } } else if (m_attachedRotationTo != null) { m_currentCameraMatrix = m_attachedRotationOffset * m_attachedRotationTo.WorldMatrix; } else if (m_waypoints.Count > 2) { float segmentTime = 1f / (m_waypoints.Count - 1); int segment = (int)Math.Floor(timeRatio / segmentTime); if (segment > m_waypoints.Count - 2) { segment = m_waypoints.Count - 2; } float segmentRatio = (timeRatio - segment * segmentTime) / segmentTime; QuaternionD quat1 = QuaternionD.CreateFromRotationMatrix(m_waypoints[segment]); QuaternionD quat2 = QuaternionD.CreateFromRotationMatrix(m_waypoints[segment + 1]); QuaternionD res = QuaternionD.Slerp(quat1, quat2, MathHelper.SmoothStepStable((double)segmentRatio)); m_currentCameraMatrix = MatrixD.CreateFromQuaternion(res); } else if (!m_nodeStartMatrix.EqualsFast(ref m_nodeEndMatrix)) { QuaternionD quat1 = QuaternionD.CreateFromRotationMatrix(m_nodeStartMatrix); QuaternionD quat2 = QuaternionD.CreateFromRotationMatrix(m_nodeEndMatrix); QuaternionD res = QuaternionD.Slerp(quat1, quat2, MathHelper.SmoothStepStable((double)timeRatio)); m_currentCameraMatrix = MatrixD.CreateFromQuaternion(res); } m_currentCameraMatrix.Translation = newPos; //FOV if (m_currentNode.ChangeFOVTo > MINIMUM_FOV) { m_currentFOV = MathHelper.SmoothStep(m_nodeStartFOV, MathHelper.Clamp(m_currentNode.ChangeFOVTo, MINIMUM_FOV, MAXIMUM_FOV), timeRatio); } m_cameraEntity.FOV = m_currentFOV; //next node if (m_currentTime >= m_currentNode.Time) { CutsceneNext(false); } }
} // Save method public void Update() { curPos = rc.GetPosition(); // get current position vector curRot = QuaternionD.CreateFromRotationMatrix(rc.WorldMatrix.GetOrientation()); // get current rotation quaternion } // Update method
private void TargetUpdated(ZACommons commons, EventDriver eventDriver, MyDetectedEntityInfo info, bool full = false, bool localOnly = false, TimeSpan?updateTime = null, bool newOffset = true) { var position = info.Position; var velocity = new Vector3D(info.Velocity); // Convert to quaternion so it's more compact var orientation = QuaternionD.CreateFromRotationMatrix(info.Orientation); if (updateTime == null) { // Fresh update, use it for gyro lock TargetPosition = position; TargetVelocity = velocity; LastTargetUpdate = eventDriver.TimeSinceStart; } else { // Interpolate position since given update time var delta = (eventDriver.TimeSinceStart - (TimeSpan)updateTime).TotalSeconds; position += velocity * delta; } // Compose message string msg; if (full) { Vector3D localOffset; if (newOffset) { // Be sure to use original position when determining offset var offset = (Vector3D)info.HitPosition - info.Position; var toLocal = MatrixD.Transpose(info.Orientation); localOffset = Vector3D.TransformNormal(offset, toLocal); // Save for future TargetOffset = localOffset; } else { localOffset = TargetOffset; } msg = string.Format("tnew;{0};{1};{2};{3};{4};{5};{6};{7};{8};{9};{10};{11};{12};{13}", info.EntityId, position.X, position.Y, position.Z, velocity.X, velocity.Y, velocity.Z, orientation.X, orientation.Y, orientation.Z, orientation.W, localOffset.X, localOffset.Y, localOffset.Z); } else { msg = string.Format("tupdate;{0};{1};{2};{3};{4};{5};{6};{7};{8};{9};{10}", info.EntityId, position.X, position.Y, position.Z, velocity.X, velocity.Y, velocity.Z, orientation.X, orientation.Y, orientation.Z, orientation.W); } var broadcasted = false; foreach (var group in commons.GetBlockGroupsWithPrefix(TARGET_UPDATE_PREFIX)) { foreach (var block in group.Blocks) { if (block is IMyProgrammableBlock) { ((IMyProgrammableBlock)block).TryRun(msg); } else if (!localOnly && block is IMyLaserAntenna) { ((IMyLaserAntenna)block).TransmitMessage(msg); } else if (!localOnly && !broadcasted && block is IMyRadioAntenna) { // Only if functional and enabled var antenna = (IMyRadioAntenna)block; if (antenna.IsFunctional && antenna.Enabled) { antenna.TransmitMessage(msg, TRACKER_ANTENNA_TARGET); broadcasted = true; } } } } }
public override void UpdateBeforeSimulation() { try { if (first) { var mod = Soil.instance; if (mod == null) { return; } first = false; // don't move this up because it needs to repeat until mod and character are available for a valid check if (MyAPIGateway.Session.Player != null) // it's null for DS and might be for other cases, we don't care for those cases { var gun = (IMyGunBaseUser)Entity; // check if the local player is holding it if (gun?.Owner != null && gun.OwnerId == MyAPIGateway.Session.Player.IdentityId) { mod.DrawTool((IMyAutomaticRifleGun)Entity); } } if (!Entity.TryGetSubpart(SUBPART_NAME, out subpart)) { NeedsUpdate = MyEntityUpdateEnum.NONE; return; } } if (subpart == null) { return; } var tool = (IMyGunBaseUser)Entity; var character = tool.Owner as IMyCharacter; if (character == null || character.Physics == null) { return; } var pos = character.PositionComp.WorldAABB.Center; if (Vector3D.DistanceSquared(pos, MyAPIGateway.Session.Camera.WorldMatrix.Translation) > VIEW_RANGE_SQ) { return; } var angAccel = Vector3.Zero; if (character.CurrentMovementState == MyCharacterMovementEnum.Flying) { var rotation = QuaternionD.CreateFromRotationMatrix(character.PositionComp.WorldMatrixRef); var deltaRotation = rotation * QuaternionD.Inverse(prevRotation); var angVel = Vector3.Zero; // MATH - courtesy of Equinox if (1 - deltaRotation.W * deltaRotation.W > 0) { angVel = 2 * Math.Acos(deltaRotation.W) * new Vector3D(deltaRotation.X, deltaRotation.Y, deltaRotation.Z) / Math.Sqrt(1 - (deltaRotation.W * deltaRotation.W)) / (1d / 60d); angAccel = (prevAngVel - angVel) * 60f; } prevAngVel = angVel; prevRotation = rotation; } else { angAccel = character.Physics.AngularAcceleration; } var wm = subpart.WorldMatrix; var subpartPos = wm.Translation; // + wm.Forward * 0.1 + wm.Left * 0.05; var accel = character.Physics.LinearAcceleration + angAccel.Cross(subpartPos - pos); if (++skipTicks > 60) { planet = MyGamePruningStructure.GetClosestPlanet(pos); } if (planet != null) { if (planet.Closed) { planet = null; } else { var gravComp = planet.Components.Get <MyGravityProviderComponent>(); if (gravComp != null) { accel -= gravComp.GetWorldGravity(pos); } } } var dot = accel.Dot(Entity.WorldMatrix.Forward) / 20f; // how much acceleration is in the tool's forward axis torque += dot; torque *= 0.9f; // drag currentAngle += torque; // physical limits of the rotation if (currentAngle < 0) { currentAngle = 0; torque = Math.Abs(dot); // bounce } else if (currentAngle > MAX_ANGLE) { currentAngle = MAX_ANGLE; torque = -Math.Abs(dot); // bounce } var m = subpart.PositionComp.LocalMatrixRef; var rm = Matrix.CreateFromAxisAngle(m.Up, MathHelper.ToRadians(currentAngle)); rm.Translation = m.Translation; subpart.PositionComp.LocalMatrixRef.SetFrom(rm); } catch (Exception e) { Log.Error(e); } }