public Position Inverse()
{
return(new Position(
rot.Inverse().Xform(-pos),
rot.Inverse()
));
}
//Apply the hinge rotation limit
private Quat LimitHinge(Quat rotation)
{
// If limit is zero return rotation fixed to axis
if (min == 0 && max == 0 && useLimits)
{
return(Quat.AngleAxis(0, axis));
}
// Get 1 degree of freedom rotation along axis
Quat free1DOF = Limit1DOF(rotation, axis);
if (!useLimits)
{
return(free1DOF);
}
// Get offset from last rotation in angle-axis representation
Quat addR = free1DOF * Quat.Inverse(lastRotation);
float addA = Quat.Angle(Quat.identity, addR);
Vec3 secondaryAxis = new Vec3(axis.z, axis.x, axis.y);
Vec3 cross = secondaryAxis.CrossProduct(axis);
if (cross.DotProduct(addR * secondaryAxis) > 0f)
{
addA = -addA;
}
// Clamp to limits
lastAngle = Mathf.Clamp(lastAngle + addA, min, max);
return(Quat.AngleAxis(lastAngle, axis));
}
public static Quat Slerping(Quat lhs, Quat rhs, float t)
{
t = Mathf.Clamp(t, 0.0f, 1.0f);
Quat d = rhs * lhs.Inverse();
Vector4 AxisAngle = d.GetAxisAngle();
Quat dt = new Quat(AxisAngle.w * t, new MyVector3(AxisAngle.x, AxisAngle.y, AxisAngle.z));
return(dt * lhs);
}
public static Quat SLERP(Quat q, Quat r, float t)
{
t = Mathf.Clamp(t, 0.0f, 1.0f);
Quat d = r * q.Inverse();
Vector4 AxisAngle = d.GetAxisAngle();
Quat dT = new Quat(AxisAngle.w * t, new Vector3(AxisAngle.x, AxisAngle.y, AxisAngle.z));
return(dT * q);
}
public static float Angle(Quat lhs, Quat rhs)
{
lhs.Normalize();
rhs.Normalize();
rhs.Inverse();
Quat q = Multiply(lhs, rhs);
return(2 * Mathf.Acos(q.w) * Mathf.Rad2Deg);
}
void orbit()
{
angle += Time.deltaTime * orbitSpeed * speedScalar;
Quat q = new Quat(angle, new Vector3(0, 1, 0));
//positionVector is used to set how far from the sun the object orbits
Vector3 p = positionVector;
Quat k = new Quat(1.0f, p);
Quat newk = q * k * q.Inverse();
Vector3 newp = newk.GetAxis();
transform.position = newp;
}
//function for applying the transformations to the object model
public void transformModel()
{
applyMouseInputs();
Vector3[] TransformedVertices = new Vector3[ModelSpaceVertices.Length];
//set quaternions to rotate by
Quat q1 = new Quat(pitchAng - lastpitchAng, new Vector3(1, 0, 0));
Quat q2 = new Quat(yawAng - lastyawAng, new Vector3(0, 1, 0));
Quat q3 = new Quat(rollAng - lastrollAng, new Vector3(0, 0, 1));
//rotate orientation quaternion by pitch yaw and roll quaternion representations;
q = ((q * q2) * q1) * q3;
for (int i = 0; i < TransformedVertices.Length; i++)
{
//rotate each vertex by the orientation from model space;
Quat K = new Quat(ModelSpaceVertices[i]);
Quat newK = q * K * q.Inverse();
Vector3 newP = newK.GetAxis();
Vector3 quatVert = newP;
//scale each vertex by the scale multiplier;
Matrix4by4 sMatrix = scale(scaleMul);
Vector3 scaVert = sMatrix * quatVert;
//translate the object so it will be a set distance ahead of the player;
Matrix4by4 tMat = translate(new Vector3(0, 0, distanceAhead));
Vector4 ratVert = new Vector4(scaVert.x, scaVert.y, scaVert.z, 1);
Vector4 scatVert = tMat * ratVert;
//rotate the object around the origin(camera), so it is always in front of the camera;
Matrix4by4 yawMatrix = yawRot(g);
Matrix4by4 pitchMatrix = pitchRot(y);
Matrix4by4 pyMatrix = yawMatrix * pitchMatrix;
Vector3 scarotVert = pyMatrix * scatVert;
//translate the object so it is correctly positioned in the world;
Matrix4by4 tMatrix = translate(new Vector3(pos.x, 0, pos.z));
Vector4 tVert = new Vector4(scarotVert.x, scarotVert.y, scarotVert.z, 1);
Vector4 finalVert = tMatrix * tVert;
TransformedVertices[i] = finalVert;
}
//set mesh to the transformed vertices
MeshFilter MF = GetComponent <MeshFilter>();
MF.mesh.vertices = TransformedVertices;
MF.mesh.RecalculateBounds();
MF.mesh.RecalculateNormals();
//sets angles so it doesn't accumulate and increase speed over time;
lastpitchAng = pitchAng;
lastyawAng = yawAng;
lastrollAng = rollAng;
}
public static Vector3f AngularVelocity(Quat previous, Quat rotation, float deltaTime)
{
// https://www.gamedev.net/forums/topic/347752-quaternion-and-angular-velocity/
var q = rotation * Quat.Inverse(previous);
float length = Math.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
if (length <= Math.Epsilon)
{
return(new Vector3f(2f * q.x, 2f * q.y, 2f * q.z));
}
float angle = 2f * Math.Atan2(length, q.w);
return((new Vector3f(q.x, q.y, q.z)) * (angle / (length * deltaTime)));
}
// Update is called once per frame
void Update()
{
Vector3[] TransformedVertices = new Vector3[ModelSpaceVertices.Length];
angle += Time.deltaTime * 1;
Rotation *= new Quat(EulerMaths.DegtoRag(RotationSpeed), VectorMaths.VectorNormalised(new Vector3(0, 1, 0)));
for (int i = 0; i < TransformedVertices.Length; i++)
{
Quat p = new Quat(ModelSpaceVertices[i]);
Quat newp = ((Rotation * p) * Rotation.Inverse());
Vector3 newpos = newp.GetAxisAngle();
TransformedVertices[i] = newpos;
}
MeshFilter MF = GetComponent <MeshFilter>();
MF.mesh.vertices = TransformedVertices;
MF.mesh.RecalculateNormals();
MF.mesh.RecalculateBounds();
}
public static void Calculate(Vector3f top, Vector3f mid, Vector3f low, Vector3f target, Vector3f hint, Quat topRotation, Quat midRotation, ref Quat topLocalRotation, ref Quat midLocalRotation)
{
// Epsilon is used to prevent the chain to be fully extended (in order to prevent float point errors).
float epsilon = 0.0001f;
float lengthTopMid = (mid - top).length;
float lengthMidLow = (low - mid).length;
float lengthTopLow = (low - top).length;
float lengthTopTarget = Math.Clamp((target - top).length, epsilon, lengthTopMid + lengthMidLow - epsilon);
// Get angle difference using current and desired angle for mid.
// Axis is multiplied by the inverse of mid global rotation in order to make the new rotation in local space (this is done every time).
float currentMidAngle = Math.TriangleAngle(lengthTopLow, lengthTopMid, lengthMidLow);
float desiredMidAngle = Math.TriangleAngle(lengthTopTarget, lengthTopMid, lengthMidLow);
float midAngleDiff = (currentMidAngle - desiredMidAngle);
Vector3f midAxis = Vector3f.Normalize(Vector3f.Cross(top - mid, mid - low));
Quat finalMidRotation = new Quat(midAngleDiff, Quat.Inverse(midRotation) * midAxis);
midLocalRotation *= finalMidRotation;
// Rotate low in global space.
Vector3f rotatedLow = (new Quat(midAngleDiff, midAxis) * (low - mid)) + mid;
// Get angle difference by calculating the angle between top-low-target.
float topAngleDiff = Vector3f.Angle(rotatedLow - top, target - top);
Vector3f topAxis = Vector3f.Normalize(Vector3f.Cross(rotatedLow - top, target - top));
Quat finalTopRotation = new Quat(topAngleDiff, Quat.Inverse(topRotation) * topAxis);
topLocalRotation *= finalTopRotation;
// Rotate mid and low (again) in global space.
Vector3f rotatedMid = (new Quat(topAngleDiff, topAxis) * (mid - top)) + top;
Vector3f lastRotatedLow = (new Quat(topAngleDiff, topAxis) * new Quat(midAngleDiff, midAxis) * (low - mid)) + rotatedMid;
// Get a plane using top and the normal top-low (rotated).
// This will be used to calculate how many degrees needs top to be rotated in order to align mid with hint position (top and low won't be moved).
Vector3f planeNormal = Vector3f.Normalize(top - lastRotatedLow);
var plane = new Plane(planeNormal, top);
Vector3f projectedMid = plane.ClosestPoint(rotatedMid);
Vector3f projectedHint = plane.ClosestPoint(hint);
float angleDiff = Vector3f.SignedAngle(top - projectedMid, top - projectedHint, plane.normal);
Quat finalAdjustTopRotation = new Quat(angleDiff, Quat.Inverse(topRotation) * planeNormal);
topLocalRotation = finalAdjustTopRotation * topLocalRotation;
}
// Update is called once per frame
void Update()
{
t += Time.deltaTime * 0.5f;
//defining two rotations
Quat q = new Quat(Mathf.PI * 1.0f, new Vector3(0, 1, 0));
Quat r = new Quat(Mathf.PI * 0.5f, new Vector3(1, 0, 0));
//slerped value
Quat slerped = Quat.SLERP(q, r, t);
//define vector that will be rotated
Vector3 p = new Vector3(0, 8, 0);
//store that vector in a quaternion
Quat K = new Quat(p);
//newk will have new position inside
Quat newK = slerped * K * slerped.Inverse();
//get this position as avector
Vector3 newP = newK.GetAxis();
transform.position = newP;
}
//rotates the vertices of the planet model using the custom-made quaternions
void Rotate()
{
Vector3[] TransformedVertices = new Vector3[ModelSpaceVertices.Length];
rotAngle += Time.deltaTime * rotSpeed * speedScalar;
Quat Rotation = new Quat(VectorMaths.DegToRad(rotAngle), VectorMaths.VectorNormalize(new Vector3(1, 1, 1)));
for (int i = 0; i < TransformedVertices.Length; i++)
{
Quat p = new Quat(ModelSpaceVertices[i]);
Quat newp = ((Rotation * p) * Rotation.Inverse());
Vector3 newpos = newp.GetAxisAngle();
TransformedVertices[i] = newpos;
}
// TransformedVertices[0].x = -300;
MeshFilter MF = GetComponent <MeshFilter>();
MF.mesh.vertices = TransformedVertices;
MF.mesh.RecalculateNormals();
MF.mesh.RecalculateBounds();
}
void RotatePlanet() //Same concept just x it by all the vertices e.t.c
{
Vector3[] TransformedVertices = new Vector3[ModelSpaceVertices.Length];
angle += Time.deltaTime * 50;
Quat Rotation = new Quat(VectorMaths.DegToRad(angle), VectorMaths.VectorNormalize(new Vector3(1, 1, 1)));
for (int i = 0; i < TransformedVertices.Length; i++)
{
Quat p = new Quat(ModelSpaceVertices[i]);
Quat newp = ((Rotation * p) * Rotation.Inverse());
Vector3 newpos = newp.GetAxisAngle();
TransformedVertices[i] = newpos;
}
// TransformedVertices[0].x = -300;
MeshFilter MF = GetComponent <MeshFilter>();
MF.mesh.vertices = TransformedVertices;
MF.mesh.RecalculateNormals();
MF.mesh.RecalculateBounds();
}
//function for applying transformation to the demo object, rotating around to always face the player to make it easier to work out the correct orientation. functions the same as transformModel();
public void transformDemo()
{
Vector3[] TransformedVertices = new Vector3[ModelSpaceVertices.Length];
Quat q1 = new Quat(pitchAng - lastpitchAng, new Vector3(1, 0, 0));
Quat q2 = new Quat(yawAng - lastyawAng, new Vector3(0, 1, 0));
Quat q3 = new Quat(rollAng - lastrollAng, new Vector3(0, 0, 1));
q = ((q * q2) * q1) * q3;
for (int i = 0; i < TransformedVertices.Length; i++)
{
Quat K = new Quat(ModelSpaceVertices[i]);
Quat newK = q * K * q.Inverse();
Vector3 newP = newK.GetAxis();
Vector3 quatVert = newP;
Matrix4by4 sMatrix = scale(scaleMul);
Vector3 scaVert = sMatrix * quatVert;
Matrix4by4 yawMatrix = new Matrix4by4(new Vector3(Mathf.Cos(g), 0, -Mathf.Sin(g)),
new Vector3(0, 1, 0),
new Vector3(Mathf.Sin(g), 0, Mathf.Cos(g)),
Vector3.zero);
Vector3 scarotVert = yawMatrix * scaVert;
Matrix4by4 tMatrix = translate(new Vector3(pos.x, 0, pos.z));
Vector4 tVert = new Vector4(scarotVert.x, scarotVert.y, scarotVert.z, 1);
Vector4 finalVert = tMatrix * tVert;
TransformedVertices[i] = finalVert;
}
MeshFilter MF = GetComponent <MeshFilter>();
MF.mesh.vertices = TransformedVertices;
MF.mesh.RecalculateBounds();
MF.mesh.RecalculateNormals();
lastpitchAng = pitchAng;
lastyawAng = yawAng;
lastrollAng = rollAng;
}
//Returns the limited local rotation.
public Quat GetLimitedLocalRotation(Quat localRotation, out bool changed)
{
// Making sure the Rotation Limit is initiated
if (!initiated)
{
Awake();
}
// Subtracting defaultLocalRotation
Quat rotation = Quat.Inverse(defaultLocalRotation) * localRotation;
Quat limitedRotation = LimitRotation(rotation);
changed = limitedRotation != rotation;
if (!changed)
{
return(localRotation);
}
// Apply defaultLocalRotation back on
return(defaultLocalRotation * limitedRotation);
}
public static Quat Inverse(Quat q)
{
return(Quat.Inverse(q));
}
public void Quat4()
{
FQuat fq = FQuat.Euler(( Fix64 )45, ( Fix64 )(-23), ( Fix64 )(-48.88));
FQuat fq2 = FQuat.Euler(( Fix64 )23, ( Fix64 )(-78), ( Fix64 )(-132.43f));
Quat q = Quat.Euler(45, -23, -48.88f);
Quat q2 = Quat.Euler(23, -78, -132.43f);
FVec3 fv = new FVec3(12.5f, 9, 8);
FVec3 fv2 = new FVec3(1, 0, 0);
Vec3 v = new Vec3(12.5f, 9, 8);
Vec3 v2 = new Vec3(1, 0, 0);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
this._output.WriteLine(fq2.ToString());
this._output.WriteLine(q2.ToString());
Fix64 fa = FQuat.Angle(fq, fq2);
float a = Quat.Angle(q, q2);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
fq = FQuat.AngleAxis(( Fix64 )(-123.324), fv);
q = Quat.AngleAxis(-123.324f, v);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fa = FQuat.Dot(fq, fq2);
a = Quat.Dot(q, q2);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
fq = FQuat.FromToRotation(FVec3.Normalize(fv), fv2);
q = Quat.FromToRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.Lerp(fq, fq2, ( Fix64 )0.66);
q = Quat.Lerp(q, q2, 0.66f);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.Normalize(fq);
q.Normalize();
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.Inverse();
q = Quat.Inverse(q);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fv = FQuat.Orthogonal(fv);
v = Quat.Orthogonal(v);
this._output.WriteLine(fv.ToString());
this._output.WriteLine(v.ToString());
fq = FQuat.Slerp(fq, fq2, ( Fix64 )0.66);
q = Quat.Slerp(q, q2, 0.66f);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.LookRotation(FVec3.Normalize(fv), fv2);
q = Quat.LookRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.ToAngleAxis(out fa, out fv);
q.ToAngleAxis(out a, out v);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
this._output.WriteLine(fv.ToString());
this._output.WriteLine(v.ToString());
fq = fq.Conjugate();
q = q.Conjugate();
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.SetLookRotation(FVec3.Normalize(fv), fv2);
q.SetLookRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
}
public static float AngleTo(Quat from, Quat to)
{
float result = Mathf.Acos((from.Inverse() * to).Normalized().w) * 2.0f;
return(result);
}
private static void LegSolvePatch(IntPtr thisPtr, byte boolValue, IntPtr methodPtr)
{
var kneeMode = IkTweaksSettings.IktKneeMode.Value;
if (kneeMode != KneeBendNormalMode.Default && ourLastIkController.field_Private_IkType_0 == IkController.IkType.SixPoint && (thisPtr == ourCachedSolver.LeftLeg.Pointer || thisPtr == ourCachedSolver.RightLeg.Pointer))
{
var isLeftLeg = thisPtr == ourCachedSolver.LeftLeg.Pointer;
var currentLeg = isLeftLeg
? ourCachedSolver.LeftLeg
: ourCachedSolver.RightLeg;
var weight = currentLeg.bendGoalWeight;
var oldUseKneeTarget = currentLeg.vrcUseKneeTarget;
if (weight < 1)
{
Float3 newNormal;
var currentLegBones = isLeftLeg ? ourCachedSolver.LeftLegBones : ourCachedSolver.RightLegBones;
if (kneeMode == KneeBendNormalMode.Classic)
{
newNormal = (Quat)currentLeg.IKRotation * currentLeg.vrcBendNormalRelToFoot;
}
else
{
var baseFootUpDirectionG = currentLegBones[1].solverPosition - (Float3)currentLegBones[2].solverPosition;
var baseKneeBendGoalDirectionRelToFoot = (Quat)currentLeg.rotation * Quat.Inverse(currentLegBones[2].solverRotation) * baseFootUpDirectionG;
var legRootToFoot = (Float3)currentLeg.position - (Float3)currentLegBones[0].solverPosition;
var newNormalCandidate = Float3.Cross(baseKneeBendGoalDirectionRelToFoot.normalized, legRootToFoot.normalized);
// mix between "dumb" normal and "smart" one based on cross product length;
// longer cross = presumably higher precision so mix more of "smart" normal in
var newNormalLength = newNormalCandidate.magnitude;
var baseNormal = (Quat)currentLeg.IKRotation * currentLeg.vrcBendNormalRelToFoot;
newNormal = Float3.Lerp(baseNormal.normalized, newNormalCandidate, newNormalLength).normalized;
}
// this neat little thing seemingly makes VRC recalculate the normal in some weird way
// but also... we set it ourself? whatever
currentLeg.vrcUseKneeTarget = false;
currentLeg.bendNormal = Float3.Lerp(newNormal, currentLeg.bendNormal, weight);
}
ourOriginalLegSolve(thisPtr, boolValue, methodPtr);
currentLeg.vrcUseKneeTarget = oldUseKneeTarget;
return;
}
ourOriginalLegSolve(thisPtr, boolValue, methodPtr);
}
public Vec3 VectorToLocal(Vec3 point)
{
return(Quat.Inverse(Quat.FromToRotation(Vec3.right, this.property.direction)) * point);
}
public Vec3 PointToLocal(Vec3 point)
{
return(Quat.Inverse(Quat.FromToRotation(Vec3.right, this.property.direction)) * (point - this.property.position));
}
