public FVec3 Transform(FQuat quaternion)
{
Fix64 x2 = quaternion.x + quaternion.x;
Fix64 y2 = quaternion.y + quaternion.y;
Fix64 z2 = quaternion.z + quaternion.z;
Fix64 xx2 = quaternion.x * x2;
Fix64 xy2 = quaternion.x * y2;
Fix64 xz2 = quaternion.x * z2;
Fix64 yy2 = quaternion.y * y2;
Fix64 yz2 = quaternion.y * z2;
Fix64 zz2 = quaternion.z * z2;
Fix64 wx2 = quaternion.w * x2;
Fix64 wy2 = quaternion.w * y2;
Fix64 wz2 = quaternion.w * z2;
return(new FVec3
(
this.x * (Fix64.One - yy2 - zz2) + this.y * (xy2 - wz2) + this.z * (xz2 + wy2),
this.x * (xy2 + wz2) + this.y * (Fix64.One - xx2 - zz2) + this.z * (yz2 - wx2),
this.x * (xz2 - wy2) + this.y * (yz2 + wx2) + this.z * (Fix64.One - xx2 - yy2)
));
}
public static FVec3 OrthoNormalVector(FVec3 v)
{
FVec3 res = new FVec3();
if (Fix64.Abs(v.z) > OVER_SQRT2)
{
Fix64 a = v.y * v.y + v.z * v.z;
Fix64 k = Fix64.One / Fix64.Sqrt(a);
res.x = Fix64.Zero;
res.y = -v.z * k;
res.z = v.y * k;
}
else
{
Fix64 a = v.x * v.x + v.y * v.y;
Fix64 k = Fix64.One / Fix64.Sqrt(a);
res.x = -v.y * k;
res.y = v.x * k;
res.z = Fix64.Zero;
}
return(res);
}
public static FVec3 Max(FVec3 v, Fix64 value)
{
return(new FVec3(Fix64.Max(v.x, value), Fix64.Max(v.y, value), Fix64.Max(v.z, value)));
}
public static FVec3 Hermite(FVec3 value1, FVec3 tangent1, FVec3 value2, FVec3 tangent2, Fix64 t)
{
Fix64 weightSquared = t * t;
Fix64 weightCubed = t * weightSquared;
Fix64 value1Blend = ( Fix64 )2 * weightCubed - ( Fix64 )3 * weightSquared + Fix64.One;
Fix64 tangent1Blend = weightCubed - ( Fix64 )2 * weightSquared + t;
Fix64 value2Blend = ( Fix64 )(-2) * weightCubed + ( Fix64 )3 * weightSquared;
Fix64 tangent2Blend = weightCubed - weightSquared;
return(new FVec3
(
value1.x * value1Blend + value2.x * value2Blend + tangent1.x * tangent1Blend + tangent2.x * tangent2Blend,
value1.y * value1Blend + value2.y * value2Blend + tangent1.y * tangent1Blend + tangent2.y * tangent2Blend,
value1.z * value1Blend + value2.z * value2Blend + tangent1.z * tangent1Blend + tangent2.z * tangent2Blend
));
}
public FVec3(float value)
{
this.x = ( Fix64 )value;
this.y = ( Fix64 )value;
this.z = ( Fix64 )value;
}
public static FVec3 RotateTowards(FVec3 current, FVec3 target, Fix64 maxRadiansDelta, Fix64 maxMagnitudeDelta)
{
Fix64 len1 = current.Magnitude();
Fix64 len2 = target.Magnitude();
if (len1 > Fix64.Epsilon && len2 > Fix64.Epsilon)
{
FVec3 from = current / len1;
FVec3 to = target / len2;
Fix64 cosom = Dot(from, to);
if (cosom > Fix64.One - Fix64.Epsilon)
{
return(MoveTowards(current, target, maxMagnitudeDelta));
}
if (cosom < -Fix64.One + Fix64.Epsilon)
{
FQuat q = FQuat.AngleAxis(maxRadiansDelta * Fix64.RAD_TO_DEG, OrthoNormalVector(from));
return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
}
else
{
Fix64 angle = Fix64.Acos(cosom);
FQuat q = FQuat.AngleAxis(Fix64.Min(maxRadiansDelta, angle) * Fix64.RAD_TO_DEG,
Normalize(Cross(from, to)));
return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
}
}
return(MoveTowards(current, target, maxMagnitudeDelta));
}
public static FVec3 SmoothDamp(FVec3 current, FVec3 target, ref FVec3 currentVelocity, Fix64 smoothTime, Fix64 maxSpeed,
Fix64 deltaTime)
{
smoothTime = Fix64.Max(Fix64.Epsilon, smoothTime);
Fix64 num = ( Fix64 )2 / smoothTime;
Fix64 num2 = num * deltaTime;
Fix64 num3 = Fix64.One / (Fix64.One + num2 + ( Fix64 )0.48 * num2 * num2 + ( Fix64 )0.235 * num2 * num2 * num2);
Fix64 maxLength = maxSpeed * smoothTime;
FVec3 vector = current - target;
vector.ClampMagnitude(maxLength);
target = current - vector;
FVec3 vec3 = (currentVelocity + (vector * num)) * deltaTime;
currentVelocity = (currentVelocity - (vec3 * num)) * num3;
var vector4 = target + (vector + vec3) * num3;
if (Dot(target - current, vector4 - target) > Fix64.Zero)
{
vector4 = target;
currentVelocity.Set(Fix64.Zero, Fix64.Zero, Fix64.Zero);
}
return(vector4);
}
public Fix64 Magnitude()
{
return(Fix64.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z));
}
public FVec3(Fix64 value)
{
this.x = value;
this.y = value;
this.z = value;
}
public static FVec3 Normalize(FVec3 v)
{
return(v * (Fix64.One / Fix64.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z)));
}
public bool ApproxEquals(FVec3 vector, Fix64 tolerance)
{
return(this.Distance(vector) <= tolerance);
}
public void Scale(FVec3 scale)
{
this.x *= scale.x;
this.y *= scale.y;
this.z *= scale.z;
}
public void Negate()
{
this.x = -this.x;
this.y = -this.y;
this.z = -this.z;
}
public static FVec3 Min(FVec3 v, FVec3 v1)
{
return(new FVec3(Fix64.Min(v.x, v1.x), Fix64.Min(v.y, v1.y), Fix64.Min(v.z, v1.z)));
}
public static FVec3 LerpUnclamped(FVec3 from, FVec3 to, Fix64 t)
{
return(new FVec3(from.x + (to.x - from.x) * t, from.y + (to.y - from.y) * t, from.z + (to.z - from.z) * t));
}
public FVec3(float x, float y, float z)
{
this.x = ( Fix64 )x;
this.y = ( Fix64 )y;
this.z = ( Fix64 )z;
}
public static FVec3 Lerp(FVec3 from, FVec3 to, Fix64 t)
{
return(t <= Fix64.Zero ? from : (t >= Fix64.One ? to : LerpUnclamped(from, to, t)));
}
public FVec3(Fix64 x, Fix64 y, Fix64 z)
{
this.x = x;
this.y = y;
this.z = z;
}
public void Set(Fix64 x, Fix64 y, Fix64 z)
{
this.x = x;
this.y = y;
this.z = z;
}
