private void button4_Click(object sender, EventArgs e)
{
ClearPictureBox();
// Setup Orig Vector
MyVector origVector = new MyVector(9, 0, 0);
DrawVector(origVector, Color.Silver);
MyQuaternion multiRotationQuat = new MyQuaternion(new MyVector(0, 0, 0), Utility3D.GetDegreesToRadians(0));
// Rotate around Z
MyQuaternion anotherRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(1));
//List<double> lengths = new List<double>();
for (int outerCntr = 1; outerCntr <= 100000; outerCntr++)
{
//lengths.Add(multiRotationQuat.GetMagnitude());
for (int innerCntr = 1; innerCntr <= 360; innerCntr++)
{
multiRotationQuat = MyQuaternion.Multiply(anotherRotationQuat, multiRotationQuat);
}
//multiRotationQuat.BecomeUnitQuaternion();
}
// Draw the final output
MyVector subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
DrawVector(subRotation, Color.Yellow);
}
/// <summary>
/// Transforms an array of normalized planes by a quaternion rotation.
/// </summary>
/// <param name="planes">The array of normalized planes to transform.</param>
/// <param name="rotation">The quaternion rotation.</param>
/// <exception cref="ArgumentNullException">Thrown when <paramref name="planes"/> is <c>null</c>.</exception>
public static void Transform(MyPlane[] planes, ref MyQuaternion rotation)
{
if (planes == null)
{
throw new ArgumentNullException("planes");
}
float x2 = rotation.X + rotation.X;
float y2 = rotation.Y + rotation.Y;
float z2 = rotation.Z + rotation.Z;
float wx = rotation.W * x2;
float wy = rotation.W * y2;
float wz = rotation.W * z2;
float xx = rotation.X * x2;
float xy = rotation.X * y2;
float xz = rotation.X * z2;
float yy = rotation.Y * y2;
float yz = rotation.Y * z2;
float zz = rotation.Z * z2;
for (int i = 0; i < planes.Length; ++i)
{
float x = planes[i].Normal.X;
float y = planes[i].Normal.Y;
float z = planes[i].Normal.Z;
/*
* Note:
* Factor common arithmetic out of loop.
*/
planes[i].Normal.X = ((x * ((1.0f - yy) - zz)) + (y * (xy - wz))) + (z * (xz + wy));
planes[i].Normal.Y = ((x * (xy + wz)) + (y * ((1.0f - xx) - zz))) + (z * (yz - wx));
planes[i].Normal.Z = ((x * (xz - wy)) + (y * (yz + wx))) + (z * ((1.0f - xx) - yy));
}
}
void Start()
{
Debug.Log("rotation Vector:" + rotationVector.ToString() + "\nAngle: " + angle);
if (vectorToRotate == null)
{
vectorToRotate = new Vector3(0, 0, 0);
}
mesh = ((MeshFilter)GameObject.CreatePrimitive(PrimitiveType.Cube).GetComponent("MeshFilter")).mesh;
if (mesh == null)
{
Debug.Log("No Mesh in Cube");
}
rigid = new MyRigid(mesh);
angle = angle / 60;
q = new MyQuaternion(rotationVector, angle);
rigid.rotate(q);
Debug.Log("Original Vector: " + vectorToRotate);
Debug.Log("Rotated Point: " + q.rotate(vectorToRotate));
Graphics.DrawMesh(rigid.getMesh(), Matrix4x4.identity, material, 0);
}
/// <summary>
/// Transforms a normalized plane by a quaternion rotation.
/// </summary>
/// <param name="plane">The normalized source plane.</param>
/// <param name="rotation">The quaternion rotation.</param>
/// <returns>The transformed plane.</returns>
public static MyPlane Transform(MyPlane plane, MyQuaternion rotation)
{
MyPlane result;
float x2 = rotation.X + rotation.X;
float y2 = rotation.Y + rotation.Y;
float z2 = rotation.Z + rotation.Z;
float wx = rotation.W * x2;
float wy = rotation.W * y2;
float wz = rotation.W * z2;
float xx = rotation.X * x2;
float xy = rotation.X * y2;
float xz = rotation.X * z2;
float yy = rotation.Y * y2;
float yz = rotation.Y * z2;
float zz = rotation.Z * z2;
float x = plane.Normal.X;
float y = plane.Normal.Y;
float z = plane.Normal.Z;
result.Normal.X = ((x * ((1.0f - yy) - zz)) + (y * (xy - wz))) + (z * (xz + wy));
result.Normal.Y = ((x * (xy + wz)) + (y * ((1.0f - xx) - zz))) + (z * (yz - wx));
result.Normal.Z = ((x * (xz - wy)) + (y * (yz + wx))) + (z * ((1.0f - xx) - yy));
result.D = plane.D;
return(result);
}
private void SetExcercise(int index)
{
int maxExcercises = (int)Excersice.THIRD;
if (index > maxExcercises)
{
index = 0;
}
excersice = (Excersice)index;
secuencePointList.Clear();
switch (excersice)
{
case Excersice.FIRST:
pointsInExcersice = 2;
break;
case Excersice.SECOND:
pointsInExcersice = 4;
break;
case Excersice.THIRD:
pointsInExcersice = 5;
break;
}
for (int i = 0; i < pointsInExcersice; i++)
{
secuencePointList.Add(Vector3.zero);
}
point1.rotation = MyQuaternion.Euler(0f, 90f, 0f);
point2.rotation = MyQuaternion.Euler(0f, 90f, 0f);
}
public static MyQuaternion operator*(MyQuaternion lq, Vector3 rv)
{
MyQuaternion rop = new MyQuaternion();
rop.w = 0;
rop.vec = rv;
return(lq * rop);
}
// public static MyQuaternion Euler(float pitch, float roll, float yaw) {
// MyQuaternion q;
// // Abbreviations for the various angular functions
// float cy = Mathf.Cos(yaw * 0.5f);
// float sy = Mathf.Sin(yaw * 0.5f);
// float cr = Mathf.Cos(roll * 0.5f);
// float sr = Mathf.Sin(roll * 0.5f);
// float cp = Mathf.Cos(pitch * 0.5f);
// float sp = Mathf.Sin(pitch * 0.5f);
//
// q.W = cy * cr * cp + sy * sr * sp;
// q.X = cy * sr * cp - sy * cr * sp;
// q.Y = cy * cr * sp + sy * sr * cp;
// q.Z = sy * cr * cp - cy * sr * sp;
// return q;
// }
public static MyQuaternion Multiply(MyQuaternion a, MyQuaternion b)
{
return(new MyQuaternion(
a.W * b.W - a.Z * b.Z - a.Y * b.Y - a.X * b.X,
a.W * b.Z + a.Z * b.W + a.Y * b.X - a.X * b.Y,
a.W * b.Y - a.Z * b.X + a.Y * b.W + a.X * b.Z,
a.W * b.X + a.Z * b.Y - a.Y * b.Z + a.X * b.W
));
}
public static MyQuaternion operator *(MyQuaternion q, MyQuaternion r)
{
MyQuaternion t = new MyQuaternion();
t.w = (r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z);
t.x = (r.w * q.x + r.x * q.w - r.y * q.z + r.z * q.y);
t.y = (r.w * q.y + r.x * q.z + r.y * q.w - r.z * q.x);
t.z = (r.w * q.z - r.x * q.y + r.y * q.x + r.z * q.w);
return(t);
}
public MyQuaternion conjugate()
{
MyQuaternion newQ = new MyQuaternion(this);
newQ.vec.x = -vec.x;
newQ.vec.y = -vec.y;
newQ.vec.z = -vec.z;
return(newQ);
}
private void Rotate()
{
if (Data.RotationSpeed != 0)
{
MyQuaternion rotationToTarget = MyQuaternion.LookRotation(Data.RotationTarget - Data.Position);
//removeing z axis
rotationToTarget = MyQuaternion.Euler(rotationToTarget.eulerAngles.x, rotationToTarget.eulerAngles.y, Data.Rotation.eulerAngles.z);
Data.Rotation = MyQuaternion.RotateTowards(Data.Rotation, rotationToTarget, Data.RotationSpeed);
}
}
public static MyQuaternion QuatToMyQuat(Quaternion q)
{
MyQuaternion mQ = new MyQuaternion();
mQ.x = q.x;
mQ.y = q.y;
mQ.z = q.z;
mQ.w = q.w;
return(mQ);
}
public static MyQuaternion operator *(MyQuaternion q1, MyQuaternion q2)
{
MyQuaternion qRes = new MyQuaternion();
qRes.w = q2.w * q1.w - q2.x * q1.x - q2.y * q1.y - q2.z * q1.z;
qRes.x = q2.w * q1.x + q2.x * q1.w - q2.y * q1.z + q2.z * q1.y;
qRes.y = q2.w * q1.y + q2.x * q1.z + q2.y * q1.w - q2.z * q1.x;
qRes.z = q2.w * q1.z - q2.x * q1.y + q2.y * q1.x + q2.z * q1.w;
return(qRes);
}
public static AxisAngle toAxis(MyQuaternion q1, float angle)
{
AxisAngle temp;
temp.angle = Mathf.Sin(q1.w / 2);
temp.axis = new MyVector3(q1.x / Mathf.Sqrt(1 - q1.w * q1.w),
q1.y / Mathf.Sqrt(1 - q1.w * q1.w),
q1.z / Mathf.Sqrt(1 - q1.w * q1.w));
return(temp);
}
public MyQuaternion multiply(MyQuaternion q1)
{
MyQuaternion qRes = new MyQuaternion();
qRes.w = q1.w * this.w - q1.x * this.x - q1.y * this.y - q1.z * this.z;
qRes.x = q1.w * this.x + q1.x * this.w - q1.y * this.z + q1.z * this.y;
qRes.y = q1.w * this.y + q1.x * this.z + q1.y * this.w - q1.z * this.x;
qRes.z = q1.w * this.z - q1.x * this.y + q1.y * this.x + q1.z * this.w;
return(qRes);
}
public void rotate(MyQuaternion q)
{
Vector3[] verts = mesh.vertices;
Vector3[] rotated = new Vector3[verts.Length];
for (int i = 0; i < verts.Length; i++)
{
rotated[i] = q.rotate(verts[i]);
}
mesh.vertices = rotated;
}
public MyQuaternion Scale(float scale)
{
MyQuaternion clone = new MyQuaternion();
clone.x = x * scale;
clone.y = y * scale;
clone.z = z * scale;
clone.w = w * scale;
return(clone);
}
public MyQuaternion Conjugate()
{
MyQuaternion result = new MyQuaternion();
result.x = x * -1;
result.y = y * -1;
result.z = z * -1;
result.w = w;
return(result);
}
public MyQuaternion invert()
{
MyQuaternion inverted = new MyQuaternion(this);
MyQuaternion cq = this.conjugate();
inverted.vec.x = cq.vec.x / this.length();
inverted.vec.y = cq.vec.y / this.length();
inverted.vec.z = cq.vec.z / this.length();
inverted.w = cq.w / this.length();
return(inverted);
}
public void Deserialize(DeserializeEvent e)
{
Id = e.Reader.ReadInt32();
PlayerId = e.Reader.ReadInt32();
VisibleName = e.Reader.ReadString();
Type = (TypeSO)e.Reader.ReadInt32();
Position = new Vector3(e.Reader.ReadSingle(), e.Reader.ReadSingle(), e.Reader.ReadSingle());
Rotation = new MyQuaternion(e.Reader.ReadSingle(), e.Reader.ReadSingle(), e.Reader.ReadSingle(), e.Reader.ReadSingle());
Speed = e.Reader.ReadSingle();
Prefab = e.Reader.ReadString();
}
public MyQuaternion Inverse()
{
MyQuaternion p = new MyQuaternion();
Normalize();
p.x = -x;
p.y = -y;
p.z = -z;
p.w = w;
return(p);
}
//Operators
public static MyQuaternion operator *(MyQuaternion q, MyQuaternion p)
{
MyQuaternion m = new MyQuaternion();
m.w = (p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z);
m.x = (p.w * q.x + p.x * q.w - p.y * q.z + p.z * q.y);
m.y = (p.w * q.y + p.x * q.z + p.y * q.w - p.z * q.x);
m.z = (p.w * q.z - p.x * q.y + p.y * q.x + p.z * q.w);
m.Normalize();
return(m);
}
public static MyQuaternion FromAxisAngle(axisAngle axis)
{
MyQuaternion q = new MyQuaternion();
q.w = Mathf.Cos(axis.w / 2);
q.x = axis.x * Mathf.Sin(axis.w / 2);
q.y = axis.y * Mathf.Sin(axis.w / 2);
q.z = axis.z * Mathf.Sin(axis.w / 2);
q.Normalize();
return(q);
}
public static MyQuaternion Normalize(MyQuaternion q)
{
MyQuaternion result = new MyQuaternion();
float magnitude = Mathf.Sqrt((q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w));
float scale = 1.0f / magnitude;
result.x = q.x * scale;
result.y = q.y * scale;
result.z = q.z * scale;
result.w = q.w * scale;
return(result);
}
public static MyQuaternion axisAngle(Vector v, float angle)
{
angle = angle * Mathf.Deg2Rad;
v = v.normalize();
MyQuaternion q = new MyQuaternion();
q.x = v.x * Mathf.Sin(angle / 2.0f);
q.y = v.y * Mathf.Sin(angle / 2.0f);
q.z = v.z * Mathf.Sin(angle / 2.0f);
q.w = Mathf.Cos(angle / 2.0f);
return(q);
}
private static void RunTests()
{
Debug.Log(new Quaternion(0, 1, 2, 3) * new Quaternion(0, 1, 2, 3));
Debug.Log(new MyQuaternion(0, 1, 2, 3) * new MyQuaternion(0, 1, 2, 3));
Debug.Log(Quaternion.AngleAxis(90f, Vector3.forward));
Debug.Log(MyQuaternion.AngleAxis(90f, Vector3.forward));
Debug.Log(Quaternion.AngleAxis(75f, Vector3.forward + Vector3.up));
Debug.Log(MyQuaternion.AngleAxis(75f, Vector3.forward + Vector3.up));
Debug.Log(Quaternion.Euler(new Vector3(0f, 0f, 45f)));
Debug.Log(MyQuaternion.Euler(new Vector3(0f, 0f, 45f)));
}
private void Rotate()
{
if (TargetToMove != null)
{
MyQuaternion rotationToTarget = MyQuaternion.LookRotation(TargetToMove.Position - p.Position);
//removeing z axis
rotationToTarget = MyQuaternion.Euler(rotationToTarget.eulerAngles.x, rotationToTarget.eulerAngles.y, zBeforeRotation);
if (p.Rotation != rotationToTarget)
{
p.Rotation = MyQuaternion.RotateTowards(p.Rotation, rotationToTarget, p.RotationSpeed * TickDeltaTime / 1000f);
Console.WriteLine("Ship {0} , rotation {1} target {2} rotation to target {3}", p.Id, p.Rotation.eulerAngles, TargetToMove.Id, rotationToTarget.eulerAngles);
}
}
}
/// <summary>
/// This will return a transform for a model or camera so that the model/camera will be placed and oriented just like
/// the physical sphere. Just set my return to model.Transform
/// </summary>
/// <param name="modelSphere">The tranform will be applied to the model that is described by this modelSphere</param>
/// <param name="physicalSphere">This is where the model should be transformed to (the physical location)</param>
public static Transform3D GetTransfromForSlaving(Sphere modelSphere, Sphere physicalSphere)
{
Transform3DGroup retVal = new Transform3DGroup();
// Rotate
MyQuaternion rotation = modelSphere.DirectionFacing.GetAngleAroundAxis(physicalSphere.DirectionFacing);
retVal.Children.Add(new RotateTransform3D(new QuaternionRotation3D(new Quaternion(rotation.X, rotation.Y, rotation.Z, rotation.W))));
//// Translate
//MyVector offset = physicalSphere.Position - modelSphere.Position;
//retVal.Children.Add(new TranslateTransform3D(offset.X, offset.Y, offset.Z));
// Exit Function
return(retVal);
}
public static MyQuaternion operator*(MyQuaternion lq, MyQuaternion rq)
{
Vector3 v = lq.getVector();
Vector3 u = rq.getVector();
float resultW;
Vector3 resultVec;
resultW = lq.w * rq.w - Vector3.Dot(v, u);
resultVec = lq.w * u + rq.w * v - Vector3.Cross(u, v);
MyQuaternion m = new MyQuaternion();
m.w = resultW;
m.vec = resultVec;
return(m);
}
//Static Methods
public static MyQuaternion AngleAxis(float angle, ref MyVector3 axis)
{
if (axis.sqrMagnitude == 0.0f)
{
return(identity);
}
MyQuaternion result = new MyQuaternion(0, 0, 0, 1);
float radians = angle * Mathf.Deg2Rad;
radians *= 0.5f;
axis.Normalize();
axis = axis * Mathf.Sin(radians);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = Mathf.Cos(radians);
return(Normalize(result));
}
public void Move_Speed1Direction_CorrectDestination(float directionx, float directiony, float directionz, float expectedx, float expectedy, float expectedz)
{
MyQuaternion rotation = new MyQuaternion();
Vector3 direction = new Vector3(directionx, directiony, directionz);
rotation.SetLookRotation(direction);
ShipData shipData = new ShipData();
shipData.Rotation = rotation;
shipData.Speed = 1f;
Ship sut = new Ship(shipData);
sut.Tick();
Vector3 actual = sut.Data.Position;
Assert.That(actual.x, Is.EqualTo(expectedx).Within(0.1), "Wrong value in X");
Assert.That(actual.y, Is.EqualTo(expectedy).Within(0.1), "Wrong value in Y");
Assert.That(actual.z, Is.EqualTo(expectedz).Within(0.1), "Wrong value in Z");
}
