//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));
}
private void UpdateCollisionSpheres(Vec3 position, Vec3 velocity, ItemObject missileItem)
{
MissionWeapon missionWeapon = new MissionWeapon(missileItem, (ItemModifier)null, (Banner)null);
Vec3 vec3_1 = velocity;
double num1 = (double)vec3_1.Normalize();
Vec3 va = vec3_1;
double num2 = (double)va.Normalize();
float x = va.x;
float y = va.y;
float z = va.z;
float num3 = (float)((double)x * (double)position.x + (double)y * (double)position.y + (double)z * (double)position.z);
Vec3 vb = new Vec3(1f, 1f);
float num4 = (double)Math.Abs(z - 0.0f) < 1.40129846432482E-45 ? 1f : z;
vb.z = (float)(-(double)num3 - (double)x * (1.0 - (double)position.x) - (double)y * (1.0 - (double)position.y) + (double)num4 * (double)position.z) / num4;
double num5 = (double)vb.Normalize();
Vec3 vec3_2 = Vec3.CrossProduct(va, vb);
double num6 = (double)vec3_2.Normalize();
Vec3[] vec3Array = new Vec3[0];
for (int index = 0; index < 0; ++index)
{
float num7 = (float)((double)index * 360.0 / 0.0 * 3.14159274101257 / 180.0);
vec3Array[index] = va + vb * (float)Math.Sin((double)num7) * 0.1f + vec3_2 * (float)Math.Cos((double)num7) * 0.1f;
}
for (int index = 0; index < vec3Array.Length; ++index)
{
Vec3 missileDirection = vec3Array[index];
WeaponData weaponData = missionWeapon.GetWeaponData(false);
Vec3 missileCollisionPoint = Mission.Current.GetMissileCollisionPoint(position, missileDirection, velocity.Length, in weaponData);
MatrixFrame frame = this.collisionEntity[index].GetFrame();
frame.origin = missileCollisionPoint;
this.collisionEntity[index].SetFrame(ref frame);
}
}
public void HandleCursor()
{
Vec3 zero1 = Vec3.Zero;
Vec3 zero2 = Vec3.Zero;
Vec2 viewportPoint = this.MissionScreen.SceneLayer.SceneView.ScreenPointToViewportPoint(new Vec2(0.5f, 0.5f));
this.MissionScreen.CombatCamera.ViewportPointToWorldRay(ref zero1, ref zero2, viewportPoint);
PathFaceRecord currentFace = new PathFaceRecord();
Vec3 intersectionPoint;
GetCursorIntersectionPoint(ref zero1, ref zero2, out float _, out intersectionPoint, ref currentFace, BodyFlags.CommonFocusRayCastExcludeFlags);
this.MissionScreen.SceneLayer.SceneView.ProjectedMousePositionOnGround(ref intersectionPoint, false, false);
MatrixFrame frame = this._cursor.GetFrame();
frame.origin = intersectionPoint;
frame.rotation.f = this.MissionScreen.CombatCamera.Frame.rotation.f;
frame.rotation.f.z = 0.0f;
double num1 = (double)frame.rotation.f.Normalize();
frame.rotation.s = this.MissionScreen.CombatCamera.Frame.rotation.s;
frame.rotation.s.z = 0.0f;
double num2 = (double)frame.rotation.s.Normalize();
frame.rotation.u = Vec3.CrossProduct(frame.rotation.s, frame.rotation.f);
frame.rotation.RotateAboutSide(-1.570796f);
BoundingBox boundingBox = this._cursor.GetMetaMesh(0).GetBoundingBox();
float num3 = boundingBox.max.y - boundingBox.min.y;
frame.Advance(-num3);
this._cursor.SetFrame(ref frame);
this._cursor.GetFirstMesh().SetMeshRenderOrder(200);
}
public void CrossProductTest3()
{
// Arrange
Vec3 a = new Vec3(1f, 1f, 1f);
Vec3 b = new Vec3(2f, 2f, 2f);
// Act
var r = Vec3.CrossProduct(a, b);
// Assert
Assert.Equal(0f, r.X, 5);
Assert.Equal(0f, r.Y, 5);
Assert.Equal(0f, r.Z, 5);
}
public void CrossProductTest2()
{
// Arrange
Vec3 a = new Vec3(-100f, 4.23f, -5f);
Vec3 b = new Vec3(-2f, 0f, 0f);
// Act
var r = Vec3.CrossProduct(a, b);
// Assert
Assert.Equal(0f, r.X, 5);
Assert.Equal(10f, r.Y, 5);
Assert.Equal(8.46f, r.Z, 5);
}
public void CrossProductTest1()
{
// Arrange
Vec3 a = new Vec3(1f, 4f, -5f);
Vec3 b = new Vec3(-2f, 2f, 1f);
// Act
var r = Vec3.CrossProduct(a, b);
// Assert
Assert.Equal(14f, r.X, 5);
Assert.Equal(9f, r.Y, 5);
Assert.Equal(10f, r.Z, 5);
}
public static Quat RotateFromTo(Vec3 source, Vec3 target)
{
source.Normalize();
target.Normalize();
Vec3 axis = source.CrossProduct(target);
Vec3 oldLength = axis.Normalize();
if (oldLength != Vec3.zero)
{
float halfCosAngle = 0.5f * source.DotProduct(target);
float cosHalfAngle = Mathf.Sqrt(0.5f + halfCosAngle);
float sinHalfAngle = Mathf.Sqrt(0.5f - halfCosAngle);
return(new Quat(axis.x * sinHalfAngle, axis.y * sinHalfAngle, axis.z * sinHalfAngle, cosHalfAngle));
}
else
{
return(new Quat(1.0f, 0.0f, 0.0f, 0.0f));
}
}
private void UpdateMapCamera()
{
//Camera with Updated Values
MatrixFrame mapCamera = ComputeMapCamera(ref this._cameraTarget, this._cameraBearing, this._cameraElevation, this.CameraDistance, ref this._lastUsedIdealCameraTarget);
//Bools
bool positionChanged = !mapCamera.origin.NearlyEquals(this._cameraFrame.origin, 1E-05f);
bool rotationChanged = !mapCamera.rotation.NearlyEquals(this._cameraFrame.rotation, 1E-05f);
//IDK If I can currnetly Handle this?
if (rotationChanged | positionChanged)
{
Game.Current.EventManager.TriggerEvent <MainMapCameraMoveEvent>(new MainMapCameraMoveEvent(rotationChanged, positionChanged));
}
//Sets Camera Frame
this._cameraFrame = mapCamera;
float height1 = 0.0f;
//Gets Height at a Position?
this.Mission.Scene.GetHeightAtPoint(this._cameraFrame.origin.AsVec2, BodyFlags.CommonCollisionExcludeFlagsForMissile, ref height1);
float height2 = height1 + 0.5f;
///Updates Camera Left Drag
if ((double)this._cameraFrame.origin.z < (double)height2)
{
//Checks if ButtonDragginMode is On!!!!!! Breakpoint here
if (this._leftButtonDraggingMode)
{
//Does some Math Magic
Vec3 clickedPosition = this._clickedPosition;
Vec3 vec3_1 = clickedPosition - Vec3.DotProduct(clickedPosition - this._cameraFrame.origin, this._cameraFrame.rotation.s) * this._cameraFrame.rotation.s;
Vec3 va = (vec3_1 - this._cameraFrame.origin).NormalizedCopy();
Vec3 vec3_2 = vec3_1 - (this._cameraFrame.origin + new Vec3(0.0f, 0.0f, height2 - this._cameraFrame.origin.z, -1f));
Vec3 vb = vec3_2.NormalizedCopy();
Vec3 vec = Vec3.CrossProduct(va, vb);
float a = vec.Normalize();
this._cameraFrame.origin.z = height2;
this._cameraFrame.rotation.u = this._cameraFrame.rotation.u.RotateAboutAnArbitraryVector(vec, a);
ref Mat3 local = ref this._cameraFrame.rotation;
vec3_2 = Vec3.CrossProduct(this._cameraFrame.rotation.u, this._cameraFrame.rotation.s);
Vec3 vec3_3 = vec3_2.NormalizedCopy();
local.f = vec3_3;
this._cameraFrame.rotation.s = Vec3.CrossProduct(this._cameraFrame.rotation.f, this._cameraFrame.rotation.u);
Vec3 planeNormal = -Vec3.Up;
Vec3 rayDirection = -this._cameraFrame.rotation.u;
float t;
//Do I need idealCameraTarget?
if (MBMath.GetRayPlaneIntersectionPoint(ref planeNormal, ref this._idealCameraTarget, ref this._cameraFrame.origin, ref rayDirection, out t))
{
this._idealCameraTarget = this._cameraFrame.origin + rayDirection * t;
this._cameraTarget = this._idealCameraTarget;
}
// Sets cameraElevation and CameraDistance
Vec2 vec2 = this._cameraFrame.rotation.f.AsVec2;
vec2 = new Vec2(vec2.Length, this._cameraFrame.rotation.f.z);
this._cameraElevation = -vec2.RotationInRadians;
vec3_2 = this._cameraFrame.origin - this._idealCameraTarget;
this.CameraDistance = vec3_2.Length - 2f;
this._targetCameraDistance = this.CameraDistance;
this._additionalElevation = this._cameraElevation - CalculateCameraElevation(this.CameraDistance);
this._lastUsedIdealCameraTarget = this._idealCameraTarget.AsVec2;
ComputeMapCamera(ref this._cameraTarget, this._cameraBearing, this._cameraElevation, this.CameraDistance, ref this._lastUsedIdealCameraTarget);
}
else
{
float num1 = 0.4712389f;
int num2 = 0;
do
{
this._cameraElevation += (double)this._cameraFrame.origin.z < (double)height2 ? num1 : -num1;
this._additionalElevation = this._cameraElevation - CalculateCameraElevation(this.CameraDistance);
float val1_1 = 700f;
float num3 = (Campaign.Current.UseFreeCameraAtMapScreen ? 1f : 0.3f) / val1_1;
float val1_2 = 50f;
float minValue = (float)-((double)val1_1 - (double)Math.Min(val1_1, Math.Max(val1_2, this.CameraDistance))) * num3;
float maxValue = Math.Max(minValue + 1E-05f, 1.555088f - CalculateCameraElevation(this.CameraDistance));
this._additionalElevation = MBMath.ClampFloat(this._additionalElevation, minValue, maxValue);
this._cameraElevation = this._additionalElevation + CalculateCameraElevation(this.CameraDistance);
Vec2 zero = Vec2.Zero;
this._cameraFrame = ComputeMapCamera(ref this._cameraTarget, this._cameraBearing, this._cameraElevation, this.CameraDistance, ref zero);
this.Mission.Scene.GetHeightAtPoint(this._cameraFrame.origin.AsVec2, BodyFlags.CommonCollisionExcludeFlagsForMissile, ref height2);
height2 += 0.5f;
if ((double)num1 > 9.99999974737875E-05)
{
num1 *= 0.5f;
}
else
{
++num2;
}
}while ((double)num1 > 9.99999974737875E-05 || (double)this._cameraFrame.origin.z < (double)height2 && num2 < 5);
if ((double)this._cameraFrame.origin.z < (double)height2)
{
this._cameraFrame.origin.z = height2;
Vec3 planeNormal = -Vec3.Up;
Vec3 rayDirection = -this._cameraFrame.rotation.u;
float t;
if (MBMath.GetRayPlaneIntersectionPoint(ref planeNormal, ref this._idealCameraTarget, ref this._cameraFrame.origin, ref rayDirection, out t))
{
this._idealCameraTarget = this._cameraFrame.origin + rayDirection * t;
this._cameraTarget = this._idealCameraTarget;
}
this.CameraDistance = (this._cameraFrame.origin - this._idealCameraTarget).Length - 2f;
this._lastUsedIdealCameraTarget = this._idealCameraTarget.AsVec2;
ComputeMapCamera(ref this._cameraTarget, this._cameraBearing, this._cameraElevation, this.CameraDistance, ref this._lastUsedIdealCameraTarget);
}
}
}
public static MBVec3 CrossProduct(MBVec3 va, MBVec3 vb) => Vec3.CrossProduct(va, vb);
// Running the solver - all the joints are iterated through once every frame
void Update()
{
// if the target hasn't been reached
if (!done)
{
// if the Max number of tries hasn't been reached
if (tries <= Mtries)
{
// starting from the second last joint (the last being the end effector)
// going back up to the root
for (int i = joints.Length - 2; i >= 0; i--)
{
// The vector from the ith joint to the end effector
Vec3 r1 = joints[joints.Length - 1].transform.position - joints[i].transform.position;
// The vector from the ith joint to the target
//Vector3 r2 = TODO2
Vec3 r2 = tpos - new Vec3(joints[i].transform.position);
// to avoid dividing by tiny numbers
if (r1.Module() * r2.Module() <= 0.001f)
{
// cos will be 1 and sin will be 0
cos[i] = 1;
sin[i] = 0;
}
else
{
// find the components (cos i sin) using dot and cross product
cos[i] = r1.DotProduct(r2) / (r1.Module() * r2.Module());
sin[i] = r1.CrossProduct(r2).Module() / (r1.Module() * r2.Module());
}
// The axis of rotation
Vec3 axis = r1.CrossProduct(r2).Normalize();
// find the angle between r1 and r2 (and clamp values if needed avoid errors)
theta[i] = Mathf.Acos(Mathf.Max(-1, Mathf.Min(1, cos[i])));
//Optional. correct angles if needed, depending on angles invert angle if sin component is negative
if (sin[i] < 0)
{
theta[i] = -theta[i];
}
// obtain an angle value between -pi and pi, and then convert to degrees
//theta[i] = TODO8
theta[i] = (float)SimpleAngle(theta[i]) * Mathf.Rad2Deg;
// rotate the ith joint along the axis by theta degrees in the world space.
// TODO9
joints[i].transform.rotation = Quat.AngleAxis(theta[i], axis) * new Quat(joints[i].transform.rotation);
rotationLimit[i].Apply();
}
// increment tries
tries++;
}
}
// find the difference in the positions of the end effector and the target
// TODO10
//float f = (tpos - new Vec3(joints[joints.Length - 1].transform.position)).Module();
float x = Mathf.Abs(joints[joints.Length - 1].transform.position.x - targ.transform.position.x);
float y = Mathf.Abs(joints[joints.Length - 1].transform.position.y - targ.transform.position.y);
float z = Mathf.Abs(joints[joints.Length - 1].transform.position.z - targ.transform.position.z);
// if target is within reach (within epsilon) then the process is done
if (x < epsilon && y < epsilon && z < epsilon)
{
done = true;
}
// if it isn't, then the process should be repeated
else
{
done = false;
}
// the target has moved, reset tries to 0 and change tpos
if (targ.transform.position != tpos)
{
tries = 0;
tpos = targ.transform.position;
}
}