private void Start()
{
Quaternion q = transform.rotation;
Quaternion twistY, swingXZ;
QuaternionUtils.TwistSwingX(q, out twistY, out swingXZ);
float angle;
Vector3 axis;
twistY.ToAngleAxis(out angle, out axis);
Debug.Log("Twist angle: " + angle + " axis: " + axis);
if (angle == 0)
{
// Arbitrary axis
// Twist around Vector3.up
axis = Vector3.up;
}
twistY = Quaternion.AngleAxis(45, axis);
swingXZ.ToAngleAxis(out angle, out axis);
Debug.Log("SwingXZ angle: " + angle + " axis: " + axis);
if (angle != 0)
{
swingXZ = Quaternion.AngleAxis(0, axis);
}
//swingXZ = Quaternion.AngleAxis(30, axis);
transform.rotation = swingXZ * twistY;
}
void LateUpdate()
{
if (active)
{
rotAxis = plane.up;
Vector3 ToParent = (parent.position - transform.position).normalized;
Vector3 ToChild = (child.position - transform.position).normalized;
Vector3 axis = Vector3.Cross(ToParent, ToChild).normalized;
//float angle = ComputeAngle(ToParent, ToChild);
mag = (axis - rotAxis).magnitude;
if (axis != Vector3.zero && (axis - rotAxis).magnitude > threshold && (-axis - rotAxis).magnitude > threshold)
{
Vector3 projected = Vector3.ProjectOnPlane(ToChild, rotAxis);
if (drawProjection)
{
Debug.DrawLine(transform.position, transform.position + 5 * projected.normalized, Color.red);
}
axis = Vector3.Cross(ToParent, projected).normalized;
//float sign = Mathf.Sign(Vector3.Dot(axis, Vector3.Cross(ToParent, projected)));
//angle = sign * Vector3.Dot(-ToParent, projected.normalized) * Mathf.Rad2Deg;
transform.rotation = parent.rotation;
QuaternionUtils.Rotate(transform, child.position, transform.position + projected);
}
}
}
public override GenericX Extrapolate(GenericX curr, GenericX prev)
{
if (curr.type == XType.NULL)
{
Debug.Log("Extrap pos element NULL !! Try to eliminate these Davin");
return(Localized);
}
if (crusher.TRSType == TRSType.Quaternion)
{
return(new GenericX(
(extrapolation == 0) ? (Quaternion)curr : QuaternionUtils.ExtrapolateQuaternion(prev, curr, 1 + extrapolation), curr.type));
}
else
{
if (extrapolation == 0)
{
return(curr);
}
Quaternion extrapolated = QuaternionUtils.ExtrapolateQuaternion(prev, curr, 1 + extrapolation);
// Test for the rare nasty (NaN, Nan, Nan, NaN) occurance... and deal with it
if (float.IsNaN(extrapolated[0]))
{
return(curr);
}
return(new GenericX(extrapolated.eulerAngles, curr.type));
}
}
public GenericX ExtrapolateRotation(GenericX curr, GenericX prev)
{
return new GenericX(
(extrapolation == 0) ? (Quaternion)curr : QuaternionUtils.ExtrapolateQuaternion(prev, curr, 1 + extrapolation),
//(extrapolation == 0) ? (Quaternion)curr : Quaternion.SlerpUnclamped(prev, curr, 1),// + extrapolation),
curr.type);
}
public void Constrain()
{
Vector3 ToParent = (parent.position - transform.position).normalized;
Vector3 ToChild = (child.position - transform.position).normalized;
Vector3 axis = Vector3.Cross(ToParent, ToChild).normalized;
float angle = ComputeAngle(ToParent, ToChild);
if (angle == -1.0f)
{
transform.Translate(0.01f, 0.0f, 0.0f);
return;
}
if (angle > maxAngle)
{
// Extract twist
Quaternion twist, swing;
QuaternionUtils.TwistSwingY(transform.localRotation, out twist, out swing);
// Restore parent rotation before applying the constraint
transform.rotation = parent.rotation;
// Re-apply twist rotation
transform.rotation *= twist;
// Contrain rotation to only angle_degrees
transform.Rotate(axis, 180 + maxAngle, Space.World);
}
}
void LateUpdate()
{
camera.transform.localRotation =
QuaternionUtils.ClampRotationAroundXAxis(camera.transform.localRotation, MinimumX, MaximumX);
float step = GetMouseScroll() * zoomSpeed;
// We only want half steps for scroll when we are far away from the character model
// otherwise, we will see some ugly clipping
step = distance > 1f ? step * 0.5f : Mathf.Round(step);
// Apply the step offset to the distance
distance = Mathf.Clamp(distance - step, minDistance, maxDistance);
if (distance == 0f) // First Person
{
Vector3 headLocal = transform.InverseTransformPoint(headPosition);
Vector3 origin = Vector3.zero;
Vector3 offset = Vector3.zero;
origin = headLocal;
offset = firstPersonStandingOffset;
// Final Position
Vector3 target = transform.TransformPoint(origin + offset);
camera.transform.position = target;
}
else // Third Person
{
Vector3 origin = Vector3.zero;
Vector3 offsetBase = Vector3.zero;
Vector3 offsetMultiplier = Vector3.zero;
origin = originalCameraPosition;
offsetBase = thirdPersonStandingOffset;
offsetMultiplier = thirdPersonStandingOffsetMultiplier;
Vector3 target = transform.TransformPoint(origin + offsetBase + (offsetMultiplier * distance));
Vector3 newPosition = target - (camera.transform.rotation * Vector3.forward * distance);
float finalDistance = distance;
RaycastHit hit;
if (Physics.Linecast(target, newPosition, out hit, viewBlockingLayers))
{
// Find a better position, with some space added in
finalDistance = Vector3.Distance(target, hit.point) - 0.1f;
}
camera.transform.position = target - (camera.transform.rotation * Vector3.forward * finalDistance);
}
// After camera logic, handle camera owner IK
HandleIK();
}
public void RotateBlock(AffineTransform transform, float degrees, float radians)
{
this.Position = transform.Apply(this.Position);
if (this.IsWorldObjectBlock())
{
WorldEditWorldObjectBlockData worldObjectBlockData = (WorldEditWorldObjectBlockData)this.BlockData;
worldObjectBlockData.SetRotation(worldObjectBlockData.Rotation * QuaternionUtils.FromAxisAngle(Vector3.Up, radians));
this.BlockData = worldObjectBlockData;
}
else if (!this.IsPlantBlock() && !this.BlockType.Equals(typeof(EmptyBlock)))
{
if (BlockUtils.HasRotatedVariants(this.BlockType, out Type[] variants))
//public bool negateForward = false;
void LateUpdate()
{
if (active)
{
float angle;
Vector3 axis;
transform.localRotation.ToAngleAxis(out angle, out axis);
if (angle != 0)
{
Quaternion twist, swing;
switch (localForward)
{
case ForwardDir.X:
QuaternionUtils.TwistSwingX(transform.localRotation, out twist, out swing);
break;
case ForwardDir.Y:
QuaternionUtils.TwistSwingY(transform.localRotation, out twist, out swing);
break;
case ForwardDir.Z:
default:
QuaternionUtils.TwistSwingZ(transform.localRotation, out twist, out swing);
break;
}
float angleTwist;
Vector3 axisTwist;
twist.ToAngleAxis(out angleTwist, out axisTwist);
if (!float.IsNaN(axisTwist.magnitude))
{
if (angleTwist > 180)
{
angleTwist = 360 - angleTwist;
axisTwist = -axisTwist;
}
if (Mathf.Abs(angleTwist) < minAngle)
{
angleTwist = minAngle;
}
else if (Mathf.Abs(angleTwist) > maxAngle)
{
angleTwist = maxAngle;
}
transform.localRotation = swing * Quaternion.AngleAxis(angleTwist, axisTwist);
}
}
}
}
public override void Constrain()
{
if (active)
{
Debug.Assert(maxAngle >= 0 && maxAngle <= 180);
Vector3 ToParent = (parent.position - transform.position).normalized;
Debug.DrawLine(parent.position, transform.position, Color.red);
Vector3 ToChild = (child.position - transform.position).normalized;
Debug.DrawLine(child.position, transform.position, Color.blue);
Vector3 axis = Vector3.Cross(ToParent, ToChild).normalized;
Debug.DrawLine(transform.position, transform.position + axis, Color.green);
float angle = ComputeAngle(ToParent, ToChild);
if (angle == -1.0f)
{
transform.Translate(0.01f, 0.0f, 0.0f);
return;
}
Debug.Log("angle " + angle);
if (angle > maxAngle)
{
// Extract twist
Quaternion twist, swing;
QuaternionUtils.TwistSwingY(transform.localRotation, out twist, out swing);
// Restore parent rotation before applying the constraint
transform.rotation = parent.rotation;
// Re-apply twist rotation
transform.rotation *= twist;
// Contrain rotation to only angle_degrees
transform.Rotate(axis, 180 + maxAngle, Space.World);
}
}
}
public void ExtrapolateNextFrame(Frame curr, Frame prev, Frame target)
{
//target.compPos = CompressedV3.Extrapolate(curr.compPos, prev.compPos, nst.extrapolation);
target.pos = Vector3.Lerp(curr.pos, curr.pos + (curr.pos - prev.pos), nst.extrapolation);
DebugX.Log(Time.time + " " + nst.name + " <color=black>Extrapolated Missing Next Frame targ:" + target.compPos + " curr:" + curr.compPos + " prev" + prev.compPos + "</color>");
target.compPos = target.pos.CompressPos();
target.msgType = (curr.msgType == MsgType.Cust_Msg) ? MsgType.Position : curr.msgType;
//TODO: need to limit the number of extrapolation iterations can occur.
// Position Elements
target.positionsMask = curr.positionsMask;
for (int i = 0; i < target.positions.Count; i++)
{
GenericX currTarget = nst.positionElements[i].target;
GenericX prevTarget = nst.positionElements[i].snapshot;
target.positions[i] = nst.positionElements[i].Extrapolate(currTarget, prevTarget);
//i.SetBitInMask(ref target.positionsMask, false);
}
// TODO: extrapolate rotations?
target.rotationsMask = curr.rotationsMask;
for (int i = 0; i < target.rotations.Count; i++)
{
GenericX currTarget = nst.rotationElements[i].target;
GenericX prevTarget = nst.rotationElements[i].snapshot;
//Debug.Log(" ExtrapolateNextFrame " + curr.rotations[i] + " -- > " + target.rotations[i]);
target.rotations[i] = nst.rotationElements[i].ExtrapolateRotation(currTarget, prevTarget);
//target.rotations[i] = nst.rotationElements[i].Extrapolate(curr.rotations[i], prev.rotations[i]);
//TEST
target.rotations[i] = QuaternionUtils.ExtrapolateQuaternion(prevTarget, currTarget, 2f);
i.SetBitInMask(ref target.rotationsMask, true);
}
}
protected void _processSingleTurret(GunComponent gun, ComponentGroupArray <TEnemyGroup> enemies)
{
Transform gunTransform = gun.CachedTransform;
BaseGunConfig gunConfigs = gun.mConfigs;
/// this method can return null if there are no enemies within an attack zone
EnemyComponent nearestEnemy = _getNearestEnemy(gunTransform, gunConfigs.mRadius, enemies);
/// skip other logic if there are no enemies near the turret
if (nearestEnemy == null)
{
return;
}
Transform enemyTransform = nearestEnemy.CachedTransform;
/// rotate the turret towards a target
gunTransform.rotation = QuaternionUtils.LookRotationXZ(enemyTransform.position - gunTransform.position);
/// shooting logic
if (gun.mElapsedReloadingTime > gunConfigs.mReloadInterval)
{
/// create a deffered request for instantiation of a new bullet
mInstantiationBuffer.Add(new TInstantiationCommand {
mGunPosition = gun.mBulletSpawTransform.position, mGunConfigs = gunConfigs, mEnemyTargetPosition = enemyTransform.position
});
gun.mElapsedReloadingTime = 0.0f; // starts to wait for an end of a reloading cycle
return;
}
/// wait for a gun is being reloading
gun.mElapsedReloadingTime += Time.deltaTime;
}
private void RotateNormals(int index, Vector3 worldPoint)
{
var normalIndex = index / 3;
var normalVector = EditorState.CurrentSpline.GetNormal(normalIndex);
var tangentVector = EditorState.CurrentSpline.Tangents[normalIndex];
var normalAngularOffset = EditorState.CurrentSpline.NormalsAngularOffsets[normalIndex];
var normalWorldVector = EditorState.CurrentSpline.transform.TransformDirection(normalVector).normalized;
var tangentWorldVector = EditorState.CurrentSpline.transform.TransformDirection(tangentVector).normalized;
var baseHandleRotation = Quaternion.LookRotation(normalWorldVector, tangentWorldVector);
EditorGUI.BeginChangeCheck();
var rotation = Handles.DoRotationHandle(baseHandleRotation, worldPoint);
if (EditorGUI.EndChangeCheck())
{
if (!EditorState.IsRotating)
{
EditorState.LastRotation = baseHandleRotation;
EditorState.IsRotating = true;
}
Undo.RecordObject(EditorState.CurrentSpline, "Rotate Normal Vector");
var normalAngleDiff = QuaternionUtils.GetSignedAngle(EditorState.LastRotation, rotation, tangentWorldVector);
EditorState.CurrentSpline.SetNormalAngularOffset(normalIndex, normalAngularOffset + normalAngleDiff);
EditorState.LastRotation = rotation;
EditorState.WasSplineModified = true;
}
else if ((EditorState.IsRotating && Event.current.type == EventType.Used) || Event.current.type == EventType.ValidateCommand)
{
EditorState.LastRotation = baseHandleRotation;
EditorState.IsRotating = false;
EditorState.WasSplineModified = true;
}
}
protected override void OnUpdate()
{
EnemyComponent currEnemy = null;
BaseEnemyConfig currEnemyConfig = null;
WaypointComponent currWaypoint = null;
float currSpeed = 0.0f;
float deltaTime = Time.deltaTime;
Transform enemyTransform = null;
Transform waypointTransform = null;
Vector3 dir;
float distance = 0.0f;
foreach (var entity in GetEntities <TEnemyGroup>())
{
currEnemy = entity.mEnemy;
currEnemyConfig = currEnemy.mConfigs;
currWaypoint = currEnemy.mCurrWaypoint;
currSpeed = currEnemyConfig.mSpeed * deltaTime;
enemyTransform = currEnemy.CachedTransform;
waypointTransform = currWaypoint.CachedTransform;
/// until the enemy doesn't reach the waypoint update its position
dir = waypointTransform.position - enemyTransform.position;
distance = dir.magnitude;
dir.Normalize();
/// rotate an enemy along its move direction
enemyTransform.rotation = Quaternion.RotateTowards(enemyTransform.rotation, QuaternionUtils.LookRotationXZ(dir), currEnemyConfig.mRotationSpeed * deltaTime);
if (distance > 0.1f) /// epsilon
{
enemyTransform.position += dir * currSpeed;
continue;
}
if (currWaypoint.mNextWaypoint != null)
{
currEnemy.mCurrWaypoint = currWaypoint.mNextWaypoint;
}
}
}
// Apply constraints after Update
void LateUpdate()
{
int jointLevel = 0;
Transform T = transform;
Transform refT = reference;
Quaternion q = QuaternionUtils.RelativeRotation(rotations[jointLevel], refT.localRotation);
Vector3 axis;
float angle;
q.ToAngleAxis(out angle, out axis);
//Debug.Log("Angle: " + angle + " Axis: " + axis);
if (angle <= constraints[jointLevel])
{
T.localRotation = rotations[0] * Quaternion.AngleAxis(angle, axis);
//T.localRotation = refT.localRotation;
}
else
{
angle = Mathf.Clamp(angle, 0, constraints[jointLevel]);
T.localRotation = rotations[0] * Quaternion.AngleAxis(angle, axis);
}
T = T.GetChild(1);
refT = refT.GetChild(1);
jointLevel++;
while (T != null && T.childCount == 2)
{
Vector3 p0 = T.position;
Vector3 p1old = T.GetChild(1).position;
Vector3 p1new = refT.GetChild(1).position;
q = QuaternionUtils.RelativeRotation(rotations[jointLevel], refT.localRotation);
q.ToAngleAxis(out angle, out axis);
angle = (float)QuaternionUtils.NormalizeAngle(angle);
//Debug.Log("Angle: " + angle + " Axis: " + axis);
// if the rotation cumplies with the constraints, apply it
if (angle <= constraints[jointLevel] && angle >= -constraints[jointLevel])
{
//T.localRotation = Quaternion.AngleAxis(angle, axis);
T.localRotation = refT.localRotation;
//T.rotation = r * T.rotation;
}
// else clamp before aplying
else
{
angle = Mathf.Clamp(angle, -constraints[jointLevel], constraints[jointLevel]);
float x = Mathf.Clamp((float)QuaternionUtils.NormalizeAngle(q.eulerAngles.x),
-constraints[jointLevel], constraints[jointLevel]);
float y = Mathf.Clamp((float)QuaternionUtils.NormalizeAngle(q.eulerAngles.y),
-constraints[jointLevel], constraints[jointLevel]);
float z = Mathf.Clamp((float)QuaternionUtils.NormalizeAngle(q.eulerAngles.z),
-constraints[jointLevel], constraints[jointLevel]);
T.localEulerAngles = new Vector3(x, y, z);
//T.localRotation = Quaternion.AngleAxis(angle, axis);
//r = Quaternion.AngleAxis(diff, axis);
//T.rotation = r * T.rotation;
}
T = T.GetChild(1);
refT = refT.GetChild(1);
jointLevel++;
}
}
public override void Update(bool isPlayerIn)
{
if (bone != null)
{
if (rotatingPartDataEntry.IsToggle)
{
bool?value = CheckConditions(isPlayerIn);
if (value.HasValue && value.Value)
{
rotating = !rotating;
}
}
else
{
bool?value = CheckConditions(isPlayerIn);
if (value.HasValue)
{
rotating = value.Value;
}
}
}
if (rotating)
{
if (hasRange)
{
{
rangePercentage += rotatingPartDataEntry.RotationSpeed * Game.FrameTime * (rangeIncreasing ? 1.0f : -1.0f);
Quaternion newRotation = QuaternionUtils.Slerp(rangeMin, rangeMax, rangePercentage, rotatingPartDataEntry.Range.LongestPath);
bone.SetRotation(newRotation);
if ((rangeIncreasing && rangePercentage >= 1.0f) ||
(!rangeIncreasing && rangePercentage <= 0.0f))
{
rangeIncreasing = !rangeIncreasing;
}
}
#if DEBUG
{
Quaternion rotation = MatrixUtils.DecomposeRotation(bone.Matrix);
Quaternion vehRot = Vehicle.Orientation;
Vector3 pos = Vehicle.GetBonePosition(bone.Index);
Debug.DrawLine(pos, pos + ((vehRot * rotation).ToVector() * 2.0f), System.Drawing.Color.Red);
Debug.DrawLine(pos, pos + ((vehRot * bone.OriginalRotation).ToVector() * 2.0f), System.Drawing.Color.Blue);
Quaternion minRot = vehRot * rangeMin;
Quaternion maxRot = vehRot * rangeMax;
Debug.DrawLine(pos, pos + (minRot.ToVector() * 2.0f), System.Drawing.Color.Green);
Debug.DrawLine(pos, pos + (maxRot.ToVector() * 2.0f), System.Drawing.Color.Purple);
}
#endif
}
else
{
Vector3 axis = rotatingPartDataEntry.RotationAxis;
float degrees = rotatingPartDataEntry.RotationSpeed * Game.FrameTime;
bone.RotateAxis(axis, degrees);
}
}
}
void Update()
{
QuaternionUtils.DrawRotationAxisFromPos(parent, transform, child, c);
}
// Apply FABRIK forward setp on the joints[i] (with constraints)
private void ForwardReachingJoint(int i)
{
// Position of the current joint
Vector3 CurrentPos = joints[i].position;
// (Imaginary) Position of where the current joint's child should be if it wasn't moved
Vector3 CurrentEndPos = CurrentPos + distanceDir[i];
Debug.Assert(Vector3.Distance(CurrentEndPos, CurrentPos) - distanceDir[i].magnitude < Mathf.Epsilon);
// (Real) Position of the current joint's child
Vector3 ChildPos = joints[i + 1].position;
// Offset between the real and the imaginary positions
//Vector3 Offset = ChildPos - CurrentEndPos;
// Distance between current joint and its child real position
float CurrentToChildDist = Vector3.Distance(CurrentPos, ChildPos);
// Distance between current joint and its imaginary child (we already stored it, and should never change!)
float RealCurrentToChildDist = distanceDir[i].magnitude;
Debug.Assert(Vector3.Distance(CurrentPos, CurrentEndPos) - RealCurrentToChildDist < Mathf.Epsilon);
// Ratio between the 2 distances above (used to linearly interpolate)
float Ratio = RealCurrentToChildDist / CurrentToChildDist;
// Linear interpolation between the current position and its child
Vector3 CurrentNewPos = (1 - Ratio) * ChildPos + Ratio * CurrentPos;
// Rotation from the old joint orientation (towards Imaginary) to the new one (towards Real)
// Ref. (0)
Vector3 OldDir = CurrentEndPos - CurrentPos;
Vector3 NewDir = ChildPos - CurrentPos;
Quaternion Rotation = Quaternion.identity;
if (Vector3.Dot(OldDir.normalized, NewDir.normalized) < 1)
{
Rotation = Quaternion.FromToRotation(OldDir.normalized, NewDir.normalized);
}
// Ref. (1)
// Move child position (Real) to where the joint thinks it should be (Imaginary)
joints[i + 1].position = CurrentEndPos;
// Store current child's rotation, to restore it later
Quaternion ChildRot = joints[i + 1].rotation;
// Ref. (2)
// Rotate current joint so that the link is looking in the direction of the child position
joints[i].rotation = Rotation * joints[i].rotation;
// Position joint in its appropriate position (on the line pointing to the child)
joints[i].position = CurrentNewPos;
// Ref. (3)
// Restore child rotation
joints[i + 1].rotation = ChildRot;
distanceDir[i] = joints[i + 1].position - joints[i].position;
if (constraints[i + 1] != null)
{
// Store child rotation
ChildRot = joints[i + 1].rotation;
// Ref. (4)
// Apply constraint (on the child!)
constraints[i + 1].Constrain();
// Compute constraint quaternion from the current child's orientation and its previous orientation
Quaternion constraint = QuaternionUtils.RelativeRotation(ChildRot, joints[i + 1].rotation);
// Ref. (5)
// Restore child orientation, since we want to apply the constraint on the current joint
joints[i + 1].rotation = ChildRot;
// Apply constraint (on the parent), it moves the children positions away from the target
joints[i].rotation = joints[i].rotation * Quaternion.Inverse(constraint);
joints[i + 1].rotation = joints[i + 1].rotation * constraint;
// Ref. (6)
// Reposition current joint so that the end effector still matches the target position
Vector3 offset = joints[joints.Length - 1].position - target.position;
joints[i].position -= offset;
}
}
/// <summary>
/// A function that returns the intermediate rotations for two spline nodes
/// </summary>
/// <param name='Q0'>
/// The rotation of the 1st spline node.
/// </param>
/// <param name='Q1'>
/// The rotation of the 2nd spline node.
/// </param>
/// <param name='Q2'>
/// In: Rotation of the 3rd spline node.
/// Out: intermediate rotation of node0.
/// </param>
/// <param name='Q3'>
/// In: Rotation of the 4th spline node.
/// Out: intermediate rotation of node1.
/// </param>
public void RecalcRotations(Quaternion Q0, Quaternion Q1, ref Quaternion Q2, ref Quaternion Q3)
{
//Recalc rotations
Q2 = QuaternionUtils.GetSquadIntermediate(Q0, Q1, Q2);
Q3 = QuaternionUtils.GetSquadIntermediate(Q1, Q2, Q3);
}
