public static Vector3 ApplyPosition(this AxisOrder target, CustomPosition relative, Vector3?current = null, float scale = 1f)
{
Vector3 newPos;
if (current != null)
{
newPos = (Vector3)current;
}
else
{
newPos = relative.position;
}
if (target.variety == SpaceVariety.OneSided)
{
foreach (AxisApplied i in target.axes)
{
newPos = relative.Translate(newPos, (Vectors.axisDirections[i.axis] * i.units) * scale, target.space);
}
}
else if (target.variety == SpaceVariety.Mixed)
{
foreach (AxisApplied i in target.axes)
{
newPos = relative.Translate(newPos, (Vectors.axisDirections[i.axis] * i.units) * scale, i.space);
}
}
return(newPos);
}
void Update()
{
#if UNITY_EDITOR
if (UnityEditor.EditorApplication.isPlaying)
{
return;
}
if (axisOrder != lastAxisOrder)
{
lastAxisOrder = axisOrder;
SetAxisOrder(axisOrder);
}
if (captureRotations)
{
captureRotations = false;
int idx = (int)axisOrder;
AxisSet thisSet = axisSets [idx];
thisSet.rotations [0] = thisSet.rings [0].localRotation;
thisSet.rotations [1] = thisSet.rings [1].localRotation;
thisSet.rotations [2] = thisSet.rings [2].localRotation;
thisSet.rotations [3] = jet.localRotation;
thisSet.jetPosition = jet.localPosition;
}
#endif
}
public static Quaternion ReverseRotation(this AxisOrder target, CustomRotation relative, Quaternion?current = null) //WORKS!
{
Quaternion newRot;
if (current != null)
{
newRot = (Quaternion)current;
}
else
{
newRot = relative.rotation;
}
if (target.variety == SpaceVariety.OneSided)
{
for (int j = target.axes.Count; j > 0; j--)
{
AxisApplied i = target.axes[j - 1];
newRot = relative.Rotate(newRot, (Vectors.axisDirections[i.axis] * -i.units), target.space);
}
}
else if (target.variety == SpaceVariety.Mixed)
{
for (int j = target.axes.Count; j > 0; j--)
{
AxisApplied i = target.axes[j - 1];
newRot = relative.Rotate(newRot, (Vectors.axisDirections[i.axis] * -i.units), i.space);
}
}
return(newRot);
}
public static Vector3 ReversePosition(this AxisOrder target, CustomPosition relative, Vector3?current = null, float scale = 1f) //takes and return GLOBAL
{
Vector3 newPos;
if (current != null)
{
newPos = (Vector3)current;
}
else
{
newPos = relative.position;
}
if (target.variety == SpaceVariety.OneSided)
{
for (int j = target.axes.Count; j > 0; j--)
{
AxisApplied i = target.axes[j - 1];
newPos = relative.Translate(newPos, -(Vectors.axisDirections[i.axis] * i.units) * scale, target.space);
}
}
else if (target.variety == SpaceVariety.Mixed)
{
for (int j = target.axes.Count; j > 0; j--)
{
AxisApplied i = target.axes[j - 1];
newPos = relative.Translate(newPos, -(Vectors.axisDirections[i.axis] * i.units) * scale, i.space);
}
}
return(newPos);
} //WORKS!
public static Quaternion ApplyRotation(this AxisOrder target, CustomRotation relative, Quaternion?current = null) //WORKS!
{
Quaternion newRot;
if (current != null)
{
newRot = (Quaternion)current;
}
else
{
newRot = relative.rotation;
}
if (target.variety == SpaceVariety.OneSided)
{
foreach (AxisApplied i in target.axes)
{
newRot = relative.Rotate(newRot, (Vectors.axisDirections[i.axis] * i.units), target.space);
}
}
else if (target.variety == SpaceVariety.Mixed)
{
foreach (AxisApplied i in target.axes)
{
newRot = relative.Rotate(newRot, (Vectors.axisDirections[i.axis] * i.units), i.space);
}
}
return(newRot);
}
//https://gamedev.stackexchange.com/questions/140579/euler-right-handed-to-quaternion-left-handed-conversion-in-unity
//direction Unity GL 右手坐标系
//----------------------------
//right x+ x-
//up y+ y+
//forward z+ z+
static public Quaternion EulerToOpenGL(Vector3 eulerAngles, AxisOrder rotationOrder)
{
// BVH's x+ axis is Unity's left (x-)
var xRot = Quaternion.AngleAxis(-eulerAngles.x, Vector3.left);
// Unity & BVH agree on the y & z axes
var yRot = Quaternion.AngleAxis(-eulerAngles.y, Vector3.up);
var zRot = Quaternion.AngleAxis(-eulerAngles.z, Vector3.forward);
switch (rotationOrder)
{
// Reproduce rotation order (no need for parentheses - it's associative)
case AxisOrder.XYZ: return(xRot * yRot * zRot);
case AxisOrder.XZY: return(xRot * zRot * yRot);
case AxisOrder.YXZ: return(yRot * xRot * zRot);
case AxisOrder.YZX: return(yRot * zRot * xRot);
case AxisOrder.ZXY: return(zRot * xRot * yRot);
case AxisOrder.ZYX: return(zRot * yRot * xRot);
}
return(Quaternion.identity);
}
public virtual void Init(
SlotModel clusterSlotModel,
T_BRAIN clusterBrain,
bool fillCluster,
AxisOrder clusterPlaneOrder)
{
planeOrder = clusterPlaneOrder;
base.Init(clusterSlotModel, clusterBrain, fillCluster);
}
public virtual void Init(
SlotModel clusterSlotModel,
ByteTrio clusterSize,
bool fillCluster,
AxisOrder clusterPlaneOrder)
{
planeOrder = clusterPlaneOrder;
base.Init(clusterSlotModel, clusterSize, fillCluster);
}
//public override void SwitchParent(Transform newParent)
//{
// if (space == Space.Self)
// {
// Vector3 originalDirection = direction;
// Vector3 originalLocalDirection = localDirection;
// //NO LINKS (compared to Position and Rotation)
// parent = newParent;
// direction = originalDirection; //Should automatically FACTOR OFFSET, no need for offset.Reverse
// }
//}
public void RemoveOffset()
{
if (space == Space.Self)
{
direction = offset.ApplyRotation(Quaternion.LookRotation(direction)) * Vector3.forward;
}
offset = new AxisOrder(null, offset.variety, offset.space);
}
public override void RemoveOffset()
{ //removes offset and moves current position ot match previous
if (space == Space.Self && link == Link.Offset)
{
position = offset.ApplyPosition(this, position);
}
offset = new AxisOrder(null, offset.variety, offset.space);
}
//public override void SwitchParent(Transform newParent)
//{
// if (space == Space.Self)
// {
// Quaternion originalRotation = rotation;
// Quaternion originalLocalRotation = localRotation;
// if (link == Link.Offset)
// {
// parent = newParent;
// rotation = offset.ReverseRotation(originalRotation);
// }
// else if (link == Link.Match)
// {
// parent = newParent;
// rotation = originalRotation;
// }
// }
//}
public override void RemoveOffset()
{
if (space == Space.Self && link == Link.Offset)
{
rotation = offset.ApplyRotation(this, rotation);
}
offset = new AxisOrder(null, offset.variety, offset.space);
}
/// <summary>
/// Create a quaternion rotation from the given euler angles, and axis order.
/// </summary>
public static QuaternionD QuaternionFromEuler(Vector3d eulerAngles, AxisOrder order)
{
QuaternionD result;
switch (order)
{
case AxisOrder.YZX:
// FIXME: use double precision arithmetic
var angles = new Vector3((float)eulerAngles.y, (float)eulerAngles.z, (float)eulerAngles.x);
var tmp = Quaternion.Euler(angles);
result = new QuaternionD(tmp.y, tmp.z, tmp.x, tmp.w);
break;
default:
throw new ArgumentException("Axis order not supported");
}
return(result);
}
public void SetAxisOrder(AxisOrder order)
{
sliderDown = -1;
sliderMouse [0] = sliderMouse [1] = sliderMouse [2] = false;
AxisSet axes = axisSets [(int)order];
rollRing = axes.rings [0];
pitchRing = axes.rings [1];
yawRing = axes.rings [2];
rollRing.GetComponent <ColorSetter> ().SetMaterial(materials [0]);
pitchRing.GetComponent <ColorSetter> ().SetMaterial(materials [1]);
yawRing.GetComponent <ColorSetter> ().SetMaterial(materials [2]);
axisOrder = order;
ResetRotations();
ResetSliders();
}
/// <summary>
/// Extract euler angles from a quarternion using the specified axis order.
/// </summary>
public static Vector3d EulerAngles(this QuaternionD q, AxisOrder order)
{
// Unity3d euler angle extraction order is ZXY
Vector3d result;
switch (order)
{
case AxisOrder.YZX:
{
// FIXME: use double precision arithmetic
var angles = new Quaternion((float)q.z, (float)q.x, (float)q.y, (float)q.w).eulerAngles;
result = new Vector3d(angles.z, angles.x, angles.y);
break;
}
default:
throw new ArgumentException("Axis order not supported");
}
// Clamp angles to range (0,360)
result.x = ClampAngle360(result.x);
result.y = ClampAngle360(result.y);
result.z = ClampAngle360(result.z);
return(result);
}
internal static extern dSpaceID dSweepAndPruneSpaceCreate(dSpaceID space, AxisOrder axisorder);
/// <summary>
/// Initializes a new instance of the <see cref="SweepAndPruneSpace"/> class
/// inside the specified space.
/// </summary>
/// <param name="space">The space which will contain the new sweep-and-prune space.</param>
/// <param name="axisOrder">The spatial axes ordering for the sweep-and-prune space.</param>
public SweepAndPruneSpace(Space space, AxisOrder axisOrder)
: base(NativeMethods.dSweepAndPruneSpaceCreate(space != null ? space.Id : dSpaceID.Null, axisOrder))
{
}
public static Vector3 ReversePosition(this AxisOrder target, CustomPosition relative, float scale = 1f)
{
return(target.ReversePosition(relative, null, scale));
}
/// <summary>
/// Initializes a new instance of the <see cref="SweepAndPruneSpace"/> class.
/// </summary>
/// <param name="axisOrder">The spatial axes ordering for the sweep-and-prune space.</param>
public SweepAndPruneSpace(AxisOrder axisOrder)
: this(null, axisOrder)
{
}
protected override int3 OnSizeChanged(ByteTrio oldSize)
{
planeOrder = m_planeOrder;
int3 diff = base.OnSizeChanged(oldSize);
int oldVolume = oldSize.Volume(), newVolume = m_size.Volume();
if (m_slotList == null)
{
m_slotList = new T_SLOT[newVolume];
return(diff);
}
if (diff.x == 0 && diff.y == 0 && diff.z == 0)
{
return(diff);
}
T_SLOT oldSlot;
T_SLOT[] oldList = m_slotList;
m_slotList = new T_SLOT[newVolume];
int
sizeX = oldSize.x,
sizeY = oldSize.y,
sizeZ = oldSize.z,
oldLineLength = sizeX,
oldPlaneLength = sizeY * oldLineLength,
x = 0, y = 0, z = 0,
index;
ByteTrio coord;
for (z = 0; z < sizeZ; z++) // volume
{
for (y = 0; y < sizeY; y++) // plane
{
for (x = 0; x < sizeX; x++) // line
{
oldSlot = oldList[x + oldLineLength * y + oldPlaneLength * z];
if (oldSlot == null)
{
continue;
}
coord = new ByteTrio(x, y, z);
index = IndexOf(coord); // Index in resized context
if (index == -1)
{
// Old slot pops out.
OnSlotRemoved(oldSlot);
oldSlot.Release();
}
else
{
// Move old slot
m_slotList[index] = oldSlot;
}
}
}
}
return(diff);
}
