// This links an SOP from a previous linkset into my linkset.
// The trick is that the SOP's position and rotation are relative to the old root SOP's
// so we are passed in the position and rotation of the old linkset so this can
// unjigger this SOP's position and rotation from the previous linkset and
// then make them relative to my linkset root.
private void LinkNonRootPart(SceneObjectPart part, Vector3 oldGroupPosition, Quaternion oldGroupRotation, int linkNum)
{
Quaternion parentRot = oldGroupRotation;
Quaternion oldRot = part.RotationOffset;
// Move our position to not be relative to the old parent
Vector3 axPos = part.OffsetPosition;
axPos *= parentRot;
part.OffsetPosition = axPos;
part.GroupPosition = oldGroupPosition + part.OffsetPosition;
part.OffsetPosition = Vector3.Zero;
// Compution our rotation to be not relative to the old parent
Quaternion worldRot = parentRot * oldRot;
part.RotationOffset = worldRot;
// Add this SOP to our linkset
part.SetParent(this);
part.ParentID = m_rootPart.LocalId;
m_parts.Add(part.UUID, part);
part.LinkNum = linkNum;
// Compute the new position of this SOP relative to the group position
part.OffsetPosition = part.GroupPosition - AbsolutePosition;
// (radams1 20120711: I don't know why part.OffsetPosition is set multiple times.
// It would have the affect of setting the physics engine position multiple
// times. In theory, that is not necessary but I don't have a good linkset
// test to know that cleaning up this code wouldn't break things.)
// Rotate the relative position by the rotation of the group
Quaternion rootRotation = m_rootPart.RotationOffset;
Vector3 pos = part.OffsetPosition;
pos *= Quaternion.Inverse(rootRotation);
part.OffsetPosition = pos;
// Compute the SOP's rotation relative to the rotation of the group.
parentRot = m_rootPart.RotationOffset;
oldRot = part.RotationOffset;
Quaternion newRot = Quaternion.Inverse(parentRot) * oldRot;
part.RotationOffset = newRot;
// Since this SOP's state has changed, push those changes into the physics engine
// and the simulator.
part.UpdatePrimFlags(UsesPhysics, IsTemporary, IsPhantom, IsVolumeDetect);
}