private void Attach(long entityID, Vector3 gearSpacePivot, CompressedPositionOrientation otherBodySpacePivot, bool force = false)
{
m_attachedEntityId = entityID;
MyEntity otherEntity;
if (MyEntities.TryGetEntityById(entityID, out otherEntity))
{
Attach(otherEntity, gearSpacePivot, otherBodySpacePivot.Matrix);
}
}
private void AttachEntity(Vector3D pivot, MyEntity otherEntity, bool force = false)
{
var gearClusterMatrix = CubeGrid.Physics.RigidBody.GetRigidBodyMatrix();
var otherClusterMatrix = otherEntity.Physics.RigidBody.GetRigidBodyMatrix();
// Calculate world (cluster) matrix of pivot
Matrix pivotCluster = gearClusterMatrix;
pivotCluster.Translation = CubeGrid.Physics.WorldToCluster(pivot);
// Convert cluser-space to local space
Vector3 gearPivotPosition = (pivotCluster * Matrix.Invert(gearClusterMatrix)).Translation;
Matrix other = pivotCluster * Matrix.Invert(otherClusterMatrix);
CompressedPositionOrientation otherPivot = new CompressedPositionOrientation(ref other);
long OtherEntity = otherEntity.EntityId;
MatrixD masterToSlave = CubeGrid.WorldMatrix * MatrixD.Invert(otherEntity.WorldMatrix);
m_attachedState.Value = new State()
{
Force = force, OtherEntityId = OtherEntity, GearPivotPosition = gearPivotPosition, OtherPivot = otherPivot, MasterToSlave = masterToSlave
};
Attach(otherEntity, gearPivotPosition, other);
}