public FormILFMatrixCalculator(RotationMatrix matrix)
{
this.m_Device = null;
this.m_Mesh = null;
this.m_Paused = false;
InitializeComponent();
this.m_FormRenderSurface = new FormILFRenderSurface();
this.panelRenderSurface.Controls.Add(this.m_FormRenderSurface);
this.m_FormRenderSurface.Visible = true;
if (!this.InitializeGraphics())
{
MessageBox.Show("Failed to initialize Direct3D.", "TRE Explorer", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
EulerAngles eulerAngles = matrix.ToEulerAngles();
this.trackBarXPitch.Value = eulerAngles.xPitch;
this.trackBarYYaw.Value = eulerAngles.yYaw;
this.trackBarZRoll.Value = eulerAngles.zRoll;
this.m_OriginalXPitch = eulerAngles.xPitch;
this.m_OriginalYYaw = eulerAngles.yYaw;
this.m_OriginalZRoll = eulerAngles.zRoll;
this.UpdateMatrix();
}
/// <summary>
/// Extracts the local Euler angles and returns a RotationMatrix
/// </summary>
/// <param name="transform"></param>
/// <returns></returns>
public static RotationMatrix EulerAnglesToRotationMatrix(this Transform transform)
{
double x = DegToRad(transform.localEulerAngles.x);
double y = DegToRad(transform.localEulerAngles.y);
double z = DegToRad(transform.localEulerAngles.z);
Matrix R1 = new Matrix(new double[, ]
{
{ Math.Cos(y), 0, Math.Sin(y) },
{ 0, 1, 0 },
{ -Math.Sin(y), 0, Math.Cos(y) }
});
Matrix R2 = new Matrix(new double[, ]
{
{ 1, 0, 0 },
{ 0, Math.Cos(x), -Math.Sin(x) },
{ 0, Math.Sin(x), Math.Cos(x) }
});
Matrix R3 = new Matrix(new double[, ]
{
{ Math.Cos(z), -Math.Sin(z), 0 },
{ Math.Sin(z), Math.Cos(z), 0 },
{ 0, 0, 1 }
});
RotationMatrix m = new RotationMatrix((R1 * R2 * R3).matrix);
return(m);
}
/// <inheritdoc />
public SingularValueDecomposition(SystemCoordinate srcListCord, SystemCoordinate destListCord) : base(srcListCord,
destListCord)
{
SourceSystemCoordinates = srcListCord;
DestinationSystemCoordinates = destListCord;
var countPoints = SourceSystemCoordinates.List.Count;
var srcMatrix = SourceSystemCoordinates.Matrix;
var dstMatrix = DestinationSystemCoordinates.Matrix;
var eMatrix = CreateEMatrix();
var cMatrix = dstMatrix.Transpose() * eMatrix * srcMatrix;
var svd = cMatrix.Svd();
var rotMatrix = (svd.U * svd.VT).Transpose(); // rotation matrix
var trace1 = cMatrix.Multiply(rotMatrix).Diagonal().Sum();
var trace2 = (srcMatrix.Transpose() * eMatrix * srcMatrix).Diagonal().Sum();
var scale = trace1 / trace2;
// reverse Helmert transformation
var dMatrixInterim = scale * (srcMatrix * rotMatrix);
var deltaMatrix = (1d / countPoints * (dstMatrix - dMatrixInterim)).Transpose();
var finalTVector = deltaMatrix.RowSums();
DeltaCoordinateMatrix = new DeltaCoordinateMatrix(finalTVector);
RotationMatrix = new RotationMatrix(rotMatrix.Transpose(), false);
M = scale - 1d;
}
private RotationMatrix EulerAnglesToRotationMatrix(double x, double y, double z)
{
x = DegToRad(x);
y = DegToRad(y);
z = DegToRad(z);
Matrix R1 = new Matrix(new double[, ]
{
{ Math.Cos(y), 0, Math.Sin(y) },
{ 0, 1, 0 },
{ -Math.Sin(y), 0, Math.Cos(y) }
});
Matrix R2 = new Matrix(new double[, ]
{
{ 1, 0, 0 },
{ 0, Math.Cos(x), -Math.Sin(x) },
{ 0, Math.Sin(x), Math.Cos(x) }
});
Matrix R3 = new Matrix(new double[, ]
{
{ Math.Cos(z), -Math.Sin(z), 0 },
{ Math.Sin(z), Math.Cos(z), 0 },
{ 0, 0, 1 }
});
RotationMatrix m = new RotationMatrix((R1 * R2 * R3).matrix);
return(m);
}
private void InitializeElementAxes()
{
var globalVectorX = new double[AXIS_COUNT];
var globalVectorY = new double[AXIS_COUNT];
var globalVectorZ = new double[AXIS_COUNT];
globalVectorX[0] = 1d;
globalVectorY[1] = 1d;
globalVectorZ[2] = 1d;
double deltaX = nodes[1].X - nodes[0].X;
double deltaY = nodes[1].Y - nodes[0].Y;
double deltaZ = nodes[1].Z - nodes[0].Z;
var elementAxis = new double[3];
elementAxis[0] = deltaX;
elementAxis[1] = deltaY;
elementAxis[2] = deltaZ;
elementAxis.ScaleIntoThis(1d / elementAxis.Norm2());
currentRotationMatrix = RotationMatrix.CalculateRotationMatrix(globalVectorX, elementAxis);
currentRotationMatrix.MultiplyIntoResult(globalVectorX, initialAxisX);
currentRotationMatrix.MultiplyIntoResult(globalVectorY, initialAxisY);
currentRotationMatrix.MultiplyIntoResult(globalVectorZ, initialAxisZ);
beamAxisX.CopyFrom(initialAxisX);
beamAxisY.CopyFrom(initialAxisY);
beamAxisZ.CopyFrom(initialAxisZ);
}
/// <inheritdoc/>
public override Point3D Deproject(Point point, Line3DElement line)
{
Point3D rotatedPoint = new Point3D(point.X / ScaleFactor, point.Y / ScaleFactor, 0);
Point3D projectedPoint = RotationMatrix.Inverse() * (CameraRotationMatrix.Inverse() * rotatedPoint);
Point3D cameraPlanePoint = projectedPoint + (Vector3D)this.Position;
NormalizedVector3D v = this.Direction;
NormalizedVector3D l = (line[1] - line[0]).Normalize();
double t;
if (v.X * l.Y - v.Y * l.X != 0)
{
t = (l.X * (cameraPlanePoint.Y - line[0].Y) - l.Y * (cameraPlanePoint.X - line[0].X)) / (v.X * l.Y - v.Y * l.X);
}
else if (v.Z * l.Y - v.Y * l.Z != 0)
{
t = (l.Z * (cameraPlanePoint.Y - line[0].Y) - l.Y * (cameraPlanePoint.Z - line[0].Z)) / (v.Z * l.Y - v.Y * l.Z);
}
else if (v.Z * l.X - v.X * l.Z != 0)
{
t = (l.Z * (cameraPlanePoint.X - line[0].X) - l.X * (cameraPlanePoint.Z - line[0].Z)) / (v.Z * l.X - v.X * l.Z);
}
else
{
throw new Exception("The lines do not intersect!");
}
Point3D pt = cameraPlanePoint + v * t;
return(pt);
}
public static GameObject MakeRobotSpawner(Grid grid, int x, int y, int health, float spawnRate,
Color colorPainted, float robotSpeed, int robotDamageDealt,
int robotHealth, int robotForwardRange, int robotSideRange,
Vector2 robotMovementDirection, Color robotColorVisible,
Vector2 robotFireDirection, RotationMatrix robotRotation,
float robotFireRate, Color robotColorPainted,
Vector2 spawnDirection)
{
GameObject spawner = GameObject.CreatePrimitive(PrimitiveType.Cube);
spawner.name = "Robot Spawner";
spawner.transform.position = new Vector3(x, y, -0.5f);
spawner.renderer.material.mainTexture = Resources.Load("Textures/robotspawner") as Texture;
spawner.renderer.material.color = Color.white;
spawner.renderer.material.shader = Shader.Find("Transparent/Diffuse");
RobotSpawner script = spawner.AddComponent<RobotSpawner>();
script.grid = grid;
script.gridCoords = new Vector2(x, y);
script.health = health;
script.spawnRate = spawnRate;
script.colorPainted = colorPainted;
script.robotSpeed = robotSpeed;
script.robotDamageDealt = robotDamageDealt;
script.robotHealth = robotHealth;
script.robotForwardRange = robotForwardRange;
script.robotSideRange = robotSideRange;
script.robotMovementDirection = robotMovementDirection;
script.robotColorVisible = robotColorVisible;
script.robotFireDirection = robotFireDirection;
script.robotRotation = robotRotation;
script.robotFireRate = robotFireRate;
script.robotColorPainted = robotColorPainted;
script.spawnDirection = spawnDirection;
return spawner;
}
public void FollowTargetOneStep(GameObject Target)
{
if (EnableInverseKinematics)
{
if (Target.transform.localPosition != lastPos || Target.transform.localRotation != lastRot)
{
lastPos = Target.transform.localPosition;
lastRot = Target.transform.localRotation;
//Inverse Kinematics
Vector r_des = Target.transform.localPosition.ToVector();
RotationMatrix C_des = Target.transform.EulerAnglesToRotationMatrix();
var result = Robot.ComputeInverseKinematics(r_des, C_des, alpha, Lambda, MaxIter, Tolerance, UseLastQAsInit ? last_q : null);
if (result.DidConverge)
{
last_q = result.q;
if (!EnablePController)
{
SetQ(result.q);
}
}
}
}
}
public void RotationMatrix_ToQuaternion_ToRotationMatrix()
{
RotationMatrix m1, m2;
Quaternion q;
Vector vecX, vecY;
int dir;
double[] r = new double[9];
for (var i = 0; i < 100; i++)
{
vecX = Vector.RandomFromDoubles(-100, 100);
vecY = Vector.RandomFromDoubles(-100, 100);
dir = Vector.CompareDirections(vecX, vecY);
m1 = new RotationMatrix(vecX, vecY);
q = m1.ToQuaternion();
m2 = q.ToRotationMatrix();
Trace.WriteLine("");
Trace.WriteLine(vecX + " " + vecY + " dir:" + dir);
Trace.WriteLine(m1);
Trace.WriteLine(q);
Trace.WriteLine(m2);
Assert.IsTrue(m1.IsSimilar(m2));
}
}
public void SimpleRotationTest()
{
this.rotation = new RotationMatrix(45, 90, 30);
Vector3 res = new Vector3(1, 2, 3);
this.rotation.Multiply(res, res);
this.AssertVectorAreEqual(new Vector3(3.4154f, 1.1554f, -1.0000f), res);
}
public void GetRotationMatrixTest()
{
this.transformation = new TransformationMatrix(1, 1, 1, 45, 90, 30);
RotationMatrix rot = this.transformation.GetRotation();
RotationMatrix expected = new RotationMatrix(45, 90, 30);
Assert.AreEqual(expected, rot);
}
/// <inheritdoc cref="Helmert"/>
/// <remarks>
/// By this constructor you can create one instance with parameters and transform
/// with different source and destination system coordinate.
/// </remarks>
public Helmert(DeltaCoordinateMatrix coordinateMatrix, RotationMatrix rotationMatrix,
double scale)
{
_deltaCoordinate = coordinateMatrix;
_rotationMatrix = rotationMatrix;
_m = scale;
_isTranformsByCoordinates = false;
}
/// <summary>
/// Converts ZXY Tait-Bryan angles to XYZ Tait-Bryan angles.
/// </summary>
/// <param name="zxyAngles">The ZXY angles in degrees</param>
/// <returns>The XYZ angles in degrees</returns>
public static Vector3 ZXYtoXYZ(Vector3 zxyAngles)
{
Quaternion q = Quaternion.Euler(zxyAngles);
RotationMatrix rm = new RotationMatrix(
q.w, q.x, q.y, q.z);
return(IRescueVec3ToUnityVec3(rm.EulerAngles));
}
public void RotationMatrix_ToQuaternion_LowTrace()
{
RotationMatrix m = new RotationMatrix(new Vector(0, 1, 0), new Vector(-1, 0, 0));
Quaternion q = m.ToQuaternion();
RotationMatrix m1 = q.ToRotationMatrix();
Assert.IsTrue(m.IsSimilar(m1));
}
public void NoRotationConstructorTest()
{
this.rotation = new RotationMatrix();
Vector3 res = new Vector3(1, 2, 3);
this.rotation.Multiply(res, res);
this.AssertVectorAreEqual(new Vector3(1, 2, 3), res);
}
public void LookAt(Vector3 location)
{
RotationMatrix = Matrix4.CreateFromQuaternion(Matrix4.LookAt(Vector3.Zero, -(location - Transformation.GetPosition()).Normalized(), new Vector3(0, 1, 0)).ExtractRotation());
ViewMatrix = RotationMatrix * Matrix4.CreateTranslation(-Transformation.GetPosition());
Transformation.SetOrientation(RotationMatrix.ExtractRotation());
Transformation.ClearModifiedFlag();
VPMatrix = Matrix4.Mult(ViewMatrix, ProjectionMatrix);
}
public Transformation(ATN.Utils.MathExt.Numerical.Coordinate pos, RotationMatrix rm)
{
position = pos;
rotationMatrix = rm;
hasMatrix = true;
}
public LPVecParams(Vector <double> dxVector, Vector <double> dyVector, Vector <double> dzVector)
{
var rotMatrixWithM = FormingRotationMatrixWithM(dxVector, dyVector, dzVector);
RotationMatrix = new RotationMatrix(rotMatrixWithM, true).Convert_RotMatrixWithM_To_RotMatrixWithoutM();
DeltaCoordinateMatrix = new DeltaCoordinateMatrix(dxVector[3], dyVector[3], dzVector[3]);
ScaleFactor = rotMatrixWithM[0, 0] - 1d;
}
public void RotationMatrix_OrthogonalOnCreation_RandomVectors()
{
RotationMatrix m;
Vector vecX, vecY;
Vector mVecX, mVecY, mVecZ;
int dir;
for (var i = 0; i < 50; i++)
{
vecX = Vector.RandomFromDoubles(-100, 100);
vecY = Vector.RandomFromDoubles(-100, 100);
m = new RotationMatrix(vecX, vecY);
Trace.WriteLine("");
Trace.WriteLine(vecX + " " + vecY);
Trace.WriteLine(m);
Assert.IsTrue(m.IsOrthogonal(), "RotationMatrix isn't orthogonal");
mVecX = new Vector(m.m00, m.m10, m.m20);
Assert.IsTrue(Vector.CompareDirections(vecX, mVecX) == 1, "Original VectorX and orthogonalized one are not parallel");
}
for (var i = 0; i < 100; i++)
{
vecX = Vector.RandomFromInts(-1, 1);
vecY = Vector.RandomFromInts(-1, 1);
dir = Vector.CompareDirections(vecX, vecY);
m = new RotationMatrix(vecX, vecY);
Trace.WriteLine("");
Trace.WriteLine(vecX + " " + vecY + " dir:" + dir);
Trace.WriteLine(m);
if (dir == 1 || dir == 3)
{
Assert.IsTrue(m.IsIdentity(), "Parallel vectors should yield an identity matrix");
}
else
{
Assert.IsTrue(m.IsOrthogonal(), "RotationMatrix isn't orthogonal");
mVecX = new Vector(m.m00, m.m10, m.m20);
Assert.IsTrue(Vector.CompareDirections(vecX, mVecX) == 1, "Original VectorX and orthogonalized X should be parallel");
mVecY = new Vector(m.m01, m.m11, m.m21);
Assert.IsTrue(Vector.CompareDirections(vecX, mVecY) == 2, "Original VectorX and orthogonalized Y should be perpendicular");
mVecZ = new Vector(m.m02, m.m12, m.m22);
Assert.IsTrue(Vector.CompareDirections(vecX, mVecZ) == 2, "Original VectorX and orthogonalized Z should be perpendicular");
Assert.IsTrue(Vector.CompareDirections(mVecX, mVecY) == 2);
Assert.IsTrue(Vector.CompareDirections(mVecX, mVecZ) == 2);
Assert.IsTrue(Vector.CompareDirections(mVecY, mVecZ) == 2);
}
}
}
public void Rotate(RotationMatrix R)
{
double x = X * R.C1R1 + Y * R.C2R1 + Z * R.C3R1;
double y = X * R.C1R2 + Y * R.C2R2 + Z * R.C3R2;
double z = X * R.C1R3 + Y * R.C2R3 + Z * R.C3R3;
X = x;
Y = y;
Z = z;
}
private void MatrixToDataGridView(RotationMatrix matrix, DataGridView dataGridView)
{
dataGridView.Rows.Clear();
for (Int32 row = 0; row < 3; row++)
{
dataGridView.Rows.Add(new Object[] { matrix[row][0], matrix[row][1], matrix[row][2] });
}
dataGridView.SelectAll();
}
public override void SetJointValue(HomogenousTransformation transformation, HomogenousTransformation nextTransformation, Link link, double q)
{
Vector p = nextTransformation.GetPosition();
RootObject.transform.localPosition = p.ToVector3();
RotationMatrix R = nextTransformation.GetRotation();
RootObject.transform.localRotation = R.ToUnityMatrix().rotation;
}
public void RotateTest()
{
Vector3 vector = new Vector3(0, 0, 90);
RotationMatrix rm = new RotationMatrix(90, 0, 0);
VectorMath.RotateVector(vector, rm);
Assert.AreEqual(0, vector.X, float.Epsilon);
Assert.AreEqual(-90, vector.Y, float.Epsilon);
Assert.AreEqual(0, vector.Z, 0.0001f);
}
/// <summary>
/// Converts XYZ Tait-Bryan angles to a <see cref="Quaternion"/>.
/// </summary>
/// <param name="xyzAngles">The XYZ angles in degrees</param>
/// <returns>The created <see cref="Quaternion"/></returns>
public static Quaternion XYZtoQuaternion(Vector3 xyzAngles)
{
RotationMatrix rotmat = new RotationMatrix(xyzAngles.x, xyzAngles.y, xyzAngles.z);
IRescue.Core.DataTypes.Vector3 forward = new IRescue.Core.DataTypes.Vector3(0, 0, 1);
IRescue.Core.DataTypes.Vector3 upward = new IRescue.Core.DataTypes.Vector3(0, 1, 0);
rotmat.Multiply(forward, forward);
rotmat.Multiply(upward, upward);
return(Quaternion.LookRotation(IRescueVec3ToUnityVec3(forward), IRescueVec3ToUnityVec3(upward)));
}
private void UpdateMatrix()
{
this.rotationMatrix = new RotationMatrix(new EulerAngles(this.yYaw, this.xPitch, this.zRoll));
MatrixToDataGridView(this.rotationMatrix, this.dataGridViewMatrix);
UpdateLabels();
this.m_FormRenderSurface.Invalidate();
}
/// <summary>
/// Test that rotations are equal. As rotations can be achieved by different rotations in
/// different orders around different axes the method creates a rotation matrix for the expected
/// and actual _values and compares the result of a multiplication with a reference vector. When
/// the output of the multiplication is the same the orientation is also similar.
/// </summary>
/// <param name="expected">The expected rotation.</param>
/// <param name="actual">The actual rotation.</param>
/// <param name="epsilon">The epsilon to use in floating point comparison.</param>
private void AssertRotationAreEqual(Vector3 expected, Vector3 actual, double epsilon)
{
RotationMatrix rotExpected = new RotationMatrix(expected.X, expected.Y, expected.Z);
RotationMatrix rotActual = new RotationMatrix(actual.X, actual.Y, actual.Z);
Vector3 vecExpected = new Vector3(1, 1, 1);
Vector3 vecActual = new Vector3(1, 1, 1);
rotExpected.Multiply(vecExpected, vecExpected);
rotActual.Multiply(vecActual, vecActual);
this.AssertVectorAreEqual(vecExpected, vecActual, epsilon);
}
// 親の基準座標系内のローカルの基準座標系の位置と方向を指定し、
// 親->ローカルへの座標変換を実行する行列をセットアップする
public void SetupParentToLocal(Vec3 pos, EulerAngles orient)
{
// 回転行列を作成する
var orientMatrix = new RotationMatrix();
orientMatrix.Setup(orient);
// 4x3行列をセットアップする
// SetupParentToLocal(Vec3, RotationMatrix)を呼ぶことで簡略化されるが処理速度は最速ではない
SetupParentToLocal(pos, orientMatrix);
}
public override void Rotate(Vector3d versor, double angle)
{
var rotationQuaternion = new Quaternion(versor, angle);
SetRotationStatus((rotationQuaternion * RotationStatus).Normalize());
SetRotationMatrix(RotationStatus.ConvertToMatrix());
SetInverseInertiaTensor(
(RotationMatrix * MassInfo.InverseBaseInertiaTensor) *
RotationMatrix.Transpose());
}
/// <summary>
/// Transform from sc1 to sc2 with one list source and destination coordinates, but many parameters.
/// </summary>
/// <exception cref="ArgumentException">Throw then you instantiate class with delta parameters,
/// and call method with transformation by parameters</exception>
public List <Point> FromSourceToDestinationByParameters(DeltaCoordinateMatrix coordinateMatrix,
RotationMatrix rotationMatrix,
double scale)
{
if (!_isTranformsByCoordinates)
{
throw new ArgumentException();
}
return(Transform(_sourceSystemCoordinate, _destinationSystemCoordinate, coordinateMatrix, rotationMatrix,
scale));
}
public void SetupLocalToParent(Vec3 pos, RotationMatrix orient)
{
// 行列の回転部分をコピーする
// 回転行列は通常、慣性空間->オブジェクト空間への行列であり、親->ローカルである
// ここではローカル->親が欲しいのでコピー中に転置している
m11 = orient.m11; m12 = orient.m21; m13 = orient.m31;
m21 = orient.m12; m22 = orient.m22; m23 = orient.m32;
m31 = orient.m13; m32 = orient.m23; m33 = orient.m33;
// 平行移動部分を設定する
tx = pos.x; ty = pos.y; tz = pos.z;
}
public void RotationMatrixTest_createByDimensionsConstructor()
{
double ang = Math.PI / 2;
RotationMatrix a = new RotationMatrix(CartesianDimension.Y, ang);
Matrix expected = new Matrix(genNodes(new double[, ]
{
{ Math.Cos(ang), 0, Math.Sin(ang) },
{ 0, 1, 0 },
{ -Math.Sin(ang), 0, Math.Cos(ang) },
}));
Assert.AreEqual(expected, a);
}
private RotationMatrix DataGridViewToMatrix(DataGridView dataGridView)
{
RotationMatrix returnValue = new RotationMatrix();
for (Int32 row = 0; row < 3; row++)
{
for (Int32 column = 0; column < 3; column++)
{
returnValue[row][column] = (Single)dataGridView.Rows[row].Cells[column].Value;
}
}
return(returnValue);
}
public RotationMatrix Multiply(RotationMatrix M1, RotationMatrix M2)
{
RotationMatrix Res = new RotationMatrix();
Res.C1R1 = M1.C1R1 * M2.C1R1 + M1.C2R1 * M2.C1R2 + M1.C3R1 * M2.C1R3;
Res.C1R2 = M1.C1R2 * M2.C1R1 + M1.C2R2 * M2.C1R2 + M1.C3R2 * M2.C1R3;
Res.C1R3 = M1.C1R3 * M2.C1R1 + M1.C2R3 * M2.C1R2 + M1.C3R3 * M2.C1R3;
Res.C2R1 = M1.C1R1 * M2.C2R1 + M1.C2R1 * M2.C2R2 + M1.C3R1 * M2.C2R3;
Res.C2R2 = M1.C1R2 * M2.C2R1 + M1.C2R2 * M2.C2R2 + M1.C3R2 * M2.C2R3;
Res.C2R3 = M1.C1R3 * M2.C2R1 + M1.C2R3 * M2.C2R2 + M1.C3R3 * M2.C2R3;
Res.C3R1 = M1.C1R1 * M2.C3R1 + M1.C2R1 * M2.C3R2 + M1.C3R1 * M2.C3R3;
Res.C3R2 = M1.C1R2 * M2.C3R1 + M1.C2R2 * M2.C3R2 + M1.C3R2 * M2.C3R3;
Res.C3R3 = M1.C1R3 * M2.C3R1 + M1.C2R3 * M2.C3R2 + M1.C3R3 * M2.C3R3;
return Res;
}
public static GameObject MakeRobot(Grid grid, int x, int y, float speed, float fireRate,
int damage, int health, int forwardRange,
int sideRange, Vector2 movementDirection,
Color colorVisible, Color colorPainted,
Vector2 fireDirection, RotationMatrix rotation)
{
GameObject robot = GameObject.CreatePrimitive(PrimitiveType.Cube);
robot.name = "Robot";
robot.transform.localScale = new Vector3(0.8f, 0.8f, 0.8f);
robot.renderer.material.mainTexture = Resources.Load("Textures/BlankBot") as Texture;
robot.renderer.material.shader = Shader.Find("Transparent/Diffuse");
robot.renderer.material.color = Color.white;
Robot script = robot.AddComponent<Robot>();
robot.transform.position = new Vector3(x, y, -0.5f);
script.colorLight = new GameObject("Light").AddComponent<Light>();
script.colorLight.transform.parent = robot.transform;
script.colorLight.transform.localPosition = new Vector3(0, 0, 0);
script.colorLight.type = LightType.Point;
script.colorLight.intensity = 1;
script.colorLight.range = 2;
script.oldPosition = robot.transform.position;
script.newPosition = robot.transform.position;
script.forwardRange = forwardRange;
script.sideRange = sideRange;
script.gridCoords = new Vector2(x, y);
script.moveSpeed = speed;
script.damageDealt = damage;
script.movementDirection = movementDirection;
script.fireDirection = fireDirection;
script.colorVisible = colorVisible;
script.colorPainted = colorPainted;
script.health = health;
script.rotation = rotation;
script.oVision = new List<Square>();
script.lastFired = Time.time;
script.fireRate = fireRate;
script.collider.enabled = true;
script.grid = grid;
script.upPlane = GameObject.CreatePrimitive(PrimitiveType.Plane);
script.upPlane.name = "Up";
script.upPlane.transform.parent = robot.transform;
script.upPlane.transform.localPosition = new Vector3(0, -1, .4f);
script.upPlane.transform.localEulerAngles = new Vector3(0, 90, 90);
script.downPlane = GameObject.CreatePrimitive(PrimitiveType.Plane);
script.downPlane.name = "Down";
script.downPlane.transform.parent = robot.transform;
script.downPlane.transform.localPosition = new Vector3(0, 1, .4f);
script.downPlane.transform.localEulerAngles = new Vector3(0, 90, 270);
script.leftPlane = GameObject.CreatePrimitive(PrimitiveType.Plane);
script.leftPlane.name = "Left";
script.leftPlane.transform.parent = robot.transform;
script.leftPlane.transform.localPosition = new Vector3(1, 0, .4f);
script.leftPlane.transform.localEulerAngles = new Vector3(90, 180, 0);
script.rightPlane = GameObject.CreatePrimitive(PrimitiveType.Plane);
script.rightPlane.name = "Right";
script.rightPlane.transform.parent = robot.transform;
script.rightPlane.transform.localPosition = new Vector3(-1, 0, .4f);
script.rightPlane.transform.localEulerAngles = new Vector3(270, 0, 0);
if(colorVisible == Color.red) {
script.upPlane.renderer.material.mainTexture = Resources.Load("Textures/redlight") as Texture;
script.downPlane.renderer.material.mainTexture = Resources.Load("Textures/redlight") as Texture;
script.leftPlane.renderer.material.mainTexture = Resources.Load("Textures/redlight") as Texture;
script.rightPlane.renderer.material.mainTexture = Resources.Load("Textures/redlight") as Texture;
}
else if(colorVisible == Color.green) {
script.upPlane.renderer.material.mainTexture = Resources.Load("Textures/greenlight") as Texture;
script.downPlane.renderer.material.mainTexture = Resources.Load("Textures/greenlight") as Texture;
script.leftPlane.renderer.material.mainTexture = Resources.Load("Textures/greenlight") as Texture;
script.rightPlane.renderer.material.mainTexture = Resources.Load("Textures/greenlight") as Texture;
}
else if(colorVisible == Color.blue) {
script.upPlane.renderer.material.mainTexture = Resources.Load("Textures/bluelight") as Texture;
script.downPlane.renderer.material.mainTexture = Resources.Load("Textures/bluelight") as Texture;
script.leftPlane.renderer.material.mainTexture = Resources.Load("Textures/bluelight") as Texture;
script.rightPlane.renderer.material.mainTexture = Resources.Load("Textures/bluelight") as Texture;
}
script.upPlane.renderer.material.shader = Shader.Find("Particles/Additive (Soft)") as Shader;
script.downPlane.renderer.material.shader = Shader.Find("Particles/Additive (Soft)") as Shader;
script.leftPlane.renderer.material.shader = Shader.Find("Particles/Additive (Soft)") as Shader;
script.rightPlane.renderer.material.shader = Shader.Find("Particles/Additive (Soft)") as Shader;
// script.upPlane.renderer.material.shader = Shader.Find("Particles/Alpha Blended") as Shader;
// script.downPlane.renderer.material.shader = Shader.Find("Particles/Alpha Blended") as Shader;
// script.leftPlane.renderer.material.shader = Shader.Find("Particles/Alpha Blended") as Shader;
// script.rightPlane.renderer.material.shader = Shader.Find("Particles/Alpha Blended") as Shader;
script.upPlane.transform.localScale = new Vector3(0, 0, 0);
script.downPlane.transform.localScale = new Vector3(0, 0, 0);
script.leftPlane.transform.localScale = new Vector3(0, 0, 0);
script.rightPlane.transform.localScale = new Vector3(0, 0, 0);
script.oldUpVisionRange = -1;
script.oldDownVisionRange = -1;
script.oldLeftVisionRange = -1;
script.oldRightVisionRange = -1;
if(script.movementDirection == new Vector2(1, 0))
script.transform.localEulerAngles = new Vector3(0, 0, 90f);
else if(script.movementDirection == new Vector2(0, 1))
script.transform.localEulerAngles = new Vector3(0, 0, 180f);
else if(script.movementDirection == new Vector2(-1, 0))
script.transform.localEulerAngles = new Vector3(0, 0, 270f);
else if(script.movementDirection == new Vector2(0, -1))
script.transform.localEulerAngles = new Vector3(0, 0, 360f);
WinChecker.numRobots++;
return robot;
}
void Update()
{
/*
* Colors panels to represent vision.
*/
int upVisionRange, downVisionRange, leftVisionRange, rightVisionRange;
nVision = new List<Square>();
RotationMatrix rot = new RotationMatrix(RotationMatrix.Rotation.Left);
Vector2 direction = fireDirection;
Vector2 lightDirection = new Vector2(0, 1);
nVision.AddRange(grid.SCheckLine(gridCoords, gridCoords + forwardRange*direction, out upVisionRange));
if(oldUpVisionRange != upVisionRange) {
ScalePlane(upPlane, upVisionRange, lightDirection);
oldUpVisionRange = upVisionRange;
}
direction = rot.Rotate(direction);
lightDirection = rot.Rotate(lightDirection);
nVision.AddRange(grid.SCheckLine(gridCoords, gridCoords + sideRange*direction, out leftVisionRange));
if(oldLeftVisionRange != leftVisionRange) {
ScalePlane(leftPlane, leftVisionRange, lightDirection);
oldLeftVisionRange = leftVisionRange;
}
direction = rot.Rotate(direction);
lightDirection = rot.Rotate(lightDirection);
nVision.AddRange(grid.SCheckLine(gridCoords, gridCoords + sideRange*direction, out downVisionRange));
if(oldDownVisionRange != downVisionRange) {
ScalePlane(downPlane, downVisionRange, lightDirection);
oldDownVisionRange = downVisionRange;
}
direction = rot.Rotate(direction);
lightDirection = rot.Rotate(lightDirection);
nVision.AddRange(grid.SCheckLine(gridCoords, gridCoords + sideRange*direction, out rightVisionRange));
if(oldRightVisionRange != rightVisionRange) {
ScalePlane(rightPlane, rightVisionRange, lightDirection);
oldRightVisionRange = rightVisionRange;
} oVision = new List<Square>();
oVision.AddRange(nVision);
if(Time.timeScale == 0)
return;
if(health <= 0) {
Instantiate(explosion, transform.position, Quaternion.identity);
AudioPlayer.PlayAudio("Audio/Effects/RobExpS", 1);
AudioPlayer.PlayAudio("Audio/Effects/RobExpE", 1);
Destroy(gameObject);
}
Fire();
if(Time.time > endMoving && moveSpeed > 0) {
Move(movementDirection);
}
AnimateMotion();
}
public void Rotate(double a, double b, double c)
{
RotationMatrix R = new RotationMatrix(a, b, c);
double x = X * R.C1R1 + Y * R.C2R1 + Z * R.C3R1;
double y = X * R.C1R2 + Y * R.C2R2 + Z * R.C3R2;
double z = X * R.C1R3 + Y * R.C2R3 + Z * R.C3R3;
X = x;
Y = y;
Z = z;
A = A + a;
B = B + b;
C = C + c;
}
public void Rotate(RotationMatrix R)
{
double x = X * R.C1R1 + Y * R.C2R1 + Z * R.C3R1;
double y = X * R.C1R2 + Y * R.C2R2 + Z * R.C3R2;
double z = X * R.C1R3 + Y * R.C2R3 + Z * R.C3R3;
double a = Math.Atan2(R.C1R2, R.C1R1) * 180 / Math.PI;
double b = Math.Atan2(-R.C1R3, Math.Sqrt(Math.Pow(R.C2R3, 2) + Math.Pow(R.C3R3, 2))) * 180 / Math.PI;
double c = Math.Atan2(R.C2R3, R.C3R3) * 180 / Math.PI;
X = x;
Y = y;
Z = z;
A = A + a;
B = B + b;
C = C + c;
}
public void Rotate(RotationMatrix R)
{
double x = Foot.X * R.C1R1 + Foot.Y * R.C2R1 + Foot.Z * R.C3R1;
double y = Foot.X * R.C1R2 + Foot.Y * R.C2R2 + Foot.Z * R.C3R2;
double z = Foot.X * R.C1R3 + Foot.Y * R.C2R3 + Foot.Z * R.C3R3;
Foot = new Point(x, y, z);
x = Tip.X * R.C1R1 + Tip.Y * R.C2R1 + Tip.Z * R.C3R1;
y = Tip.X * R.C1R2 + Tip.Y * R.C2R2 + Tip.Z * R.C3R2;
z = Tip.X * R.C1R3 + Tip.Y * R.C2R3 + Tip.Z * R.C3R3;
Tip = new Point(x, y, z);
X = Tip.X - Foot.X;
Y = Tip.Y - Foot.Y;
Z = Tip.Z - Foot.Z;
Lenght = Math.Sqrt(Math.Pow(X, 2) + Math.Pow(Y, 2) + Math.Pow(Z, 2));
}
public RotationMatrix ToRotationMatrix()
{
RotationMatrix R = new RotationMatrix();
R.C1R1 = C1R1;
R.C1R2 = C1R2;
R.C1R3 = C1R3;
R.C2R1 = C2R1;
R.C2R2 = C2R2;
R.C2R3 = C2R3;
R.C3R1 = C3R1;
R.C3R2 = C3R2;
R.C3R3 = C3R3;
return R;
}
public Point Multiply(RotationMatrix Matrix, Point Point)
{
Point Res = new Point();
Res.X = Matrix.C1R1 * Point.X + Matrix.C2R1 * Point.Y + Matrix.C3R1 * Point.Z;
Res.Y = Matrix.C1R2 * Point.X + Matrix.C2R2 * Point.Y + Matrix.C3R2 * Point.Z;
Res.Z = Matrix.C1R3 * Point.X + Matrix.C2R3 * Point.Y + Matrix.C3R3 * Point.Z;
return Res;
}
public void Rotate(double a, double b, double c)
{
RotationMatrix R = new RotationMatrix(a, b, c);
Rotate(R);
}
public void Rotate(RotationMatrix R)
{
Point point = new Point(X, Y, Z);
point.Rotate(R);
X = (int)point.X;
Y = (int)point.Y;
Z = (int)point.Z;
}
public void Rotate(RotationMatrix R)
{
List<ColorPoint> NewPoints = new List<ColorPoint>();
foreach (ColorPoint Point in Points)
{
NewPoints.Add(new ColorPoint(new Point(Point.Point.X * R.C1R1 + Point.Point.Y * R.C2R1 + Point.Point.Z * R.C3R1,
Point.Point.X * R.C1R2 + Point.Point.Y * R.C2R2 + Point.Point.Z * R.C3R2,
Point.Point.X * R.C1R3 + Point.Point.Y * R.C2R3 + Point.Point.Z * R.C3R3), Point.Color));
}
Points = NewPoints;
List<Triangle> NewMesh = new List<Triangle>();
foreach (Triangle Triangle in Mesh)
{
NewMesh.Add(new Triangle(
new Point(Triangle.Point1.X * R.C1R1 + Triangle.Point1.Y * R.C2R1 + Triangle.Point1.Z * R.C3R1,
Triangle.Point1.X * R.C1R2 + Triangle.Point1.Y * R.C2R2 + Triangle.Point1.Z * R.C3R2,
Triangle.Point1.X * R.C1R3 + Triangle.Point1.Y * R.C2R3 + Triangle.Point1.Z * R.C3R3),
new Point(Triangle.Point2.X * R.C1R1 + Triangle.Point2.Y * R.C2R1 + Triangle.Point2.Z * R.C3R1,
Triangle.Point2.X * R.C1R2 + Triangle.Point2.Y * R.C2R2 + Triangle.Point2.Z * R.C3R2,
Triangle.Point2.X * R.C1R3 + Triangle.Point2.Y * R.C2R3 + Triangle.Point2.Z * R.C3R3),
new Point(Triangle.Point3.X * R.C1R1 + Triangle.Point3.Y * R.C2R1 + Triangle.Point3.Z * R.C3R1,
Triangle.Point3.X * R.C1R2 + Triangle.Point3.Y * R.C2R2 + Triangle.Point3.Z * R.C3R2,
Triangle.Point3.X * R.C1R3 + Triangle.Point3.Y * R.C2R3 + Triangle.Point3.Z * R.C3R3)));
}
Mesh = NewMesh;
Dictionary<Key, Point> NewVoxels = new Dictionary<Key, Point>();
foreach (KeyValuePair<Key, Point> Voxel in Voxels)
{
NewVoxels.Add(Voxel.Key, new Point(Voxel.Value.X * R.C1R1 + Voxel.Value.Y * R.C2R1 + Voxel.Value.Z * R.C3R1,
Voxel.Value.X * R.C1R2 + Voxel.Value.Y * R.C2R2 + Voxel.Value.Z * R.C3R2,
Voxel.Value.X * R.C1R3 + Voxel.Value.Y * R.C2R3 + Voxel.Value.Z * R.C3R3));
}
Voxels = NewVoxels;
}
