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);
}
}
