public RegisteredCamera(float FocalLength, float k1, float k2, Matrix RotationMatrix, Vector3 TranslationVector, int ImageHeight, int ImageWidth)
{
this.FocalLength = FocalLength;
this.k1 = k1;
this.k2 = k2;
this.RotationMatrix = RotationMatrix;
this.TranslationVector = TranslationVector;
this.ImageHeight = ImageHeight;
this.ImageWidth = ImageWidth;
Double[,] aTranslationMatrix = new Double[3, 1];
aTranslationMatrix[0, 0] = TranslationVector.X;
aTranslationMatrix[1, 0] = TranslationVector.Y;
aTranslationMatrix[2, 0] = TranslationVector.Z;
Double[,] aRotationMatrix = new Double[3, 3];
aRotationMatrix[0, 0] = RotationMatrix.M11;
aRotationMatrix[0, 1] = RotationMatrix.M12;
aRotationMatrix[0, 2] = RotationMatrix.M13;
aRotationMatrix[1, 0] = RotationMatrix.M21;
aRotationMatrix[1, 1] = RotationMatrix.M22;
aRotationMatrix[1, 2] = RotationMatrix.M23;
aRotationMatrix[2, 0] = RotationMatrix.M31;
aRotationMatrix[2, 1] = RotationMatrix.M32;
aRotationMatrix[2, 2] = RotationMatrix.M33;
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseRotationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(aRotationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseTranslationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(aTranslationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix densePositionVector = (-(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Transpose()) * denseTranslationMatrix;
PositionVector = new Vector3((float)densePositionVector[0, 0], (float)densePositionVector[1, 0], (float)densePositionVector[2, 0]);
//denseTranslationMatrix = -(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Transpose() * denseTranslationMatrix;
//TranslationVector.X = (float)denseTranslationMatrix[0, 0];
//TranslationVector.Y = (float)denseTranslationMatrix[1, 0];
//TranslationVector.Z = (float)denseTranslationMatrix[2, 0];
}
public Vector3 ImageToWorld(double u, double v, double w, bool transposeRot, bool negateRot, bool invertRot, bool invertZ, float rotAngleX, float rotAngleY, float rotAngleZ)
{
double xprime = (u - ((float)(ImageWidth / 2))) / FocalLength;
double yprime = (v - ((float)(ImageHeight / 2))) / FocalLength;
//double xprime = (u) / FocalLength;
//double yprime = (v) / FocalLength;
if (invertZ)
{
w = -w;
}
double x = xprime * w;
double y = yprime * w;
double z = w;
Double[,] homoXYZ = new Double[3, 1];
homoXYZ[0, 0] = x;
homoXYZ[1, 0] = y;
homoXYZ[2, 0] = z;
Double[,] TranslationMatrix = new Double[3, 1];
TranslationMatrix[0, 0] = PositionVector.X;
TranslationMatrix[1, 0] = PositionVector.Y;
TranslationMatrix[2, 0] = PositionVector.Z;
Double[,] aRotationMatrix = new Double[3, 3];
aRotationMatrix[0, 0] = RotationMatrix.M11;
aRotationMatrix[0, 1] = RotationMatrix.M12;
aRotationMatrix[0, 2] = RotationMatrix.M13;
aRotationMatrix[1, 0] = RotationMatrix.M21;
aRotationMatrix[1, 1] = RotationMatrix.M22;
aRotationMatrix[1, 2] = RotationMatrix.M23;
aRotationMatrix[2, 0] = RotationMatrix.M31;
aRotationMatrix[2, 1] = RotationMatrix.M32;
aRotationMatrix[2, 2] = RotationMatrix.M33;
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseHomoXYZ = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(homoXYZ);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseTranslationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(TranslationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseRotationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(aRotationMatrix);
if (transposeRot)
{
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Transpose();
}
if (invertRot)
{
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Inverse();
}
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Inverse();
if (negateRot)
{
denseRotationMatrix = -denseRotationMatrix;
}
//Double[,] TranslationMatrix = new Double[3, 1];
//TranslationMatrix[0, 0] = TranslationVector.X;
//TranslationMatrix[1, 0] = TranslationVector.Y;
//TranslationMatrix[2, 0] = TranslationVector.Z;
//denseRotationMatrix = denseRotationMatrix.Transpose() *
//MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseWorldCoordinates = InverseRotationMatrix * (denseHomoXYZ - TranslationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseWorldCoordinates = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)(denseRotationMatrix) * (denseHomoXYZ - denseTranslationMatrix);
float worldX = (float)denseWorldCoordinates[0, 0];
float worldY = (float)denseWorldCoordinates[1, 0];
float worldZ = (float)denseWorldCoordinates[2, 0];
//Microsoft.Xna.Framework.Matrix rotX;
//Microsoft.Xna.Framework.Matrix rotY;
//Microsoft.Xna.Framework.Matrix rotZ;
//Vector3 origin = new Vector3(0, 0, 0);
//Microsoft.Xna.Framework.Matrix.CreateRotationX((float)rotAngleX, out rotX);
//Microsoft.Xna.Framework.Matrix.CreateRotationY((float)rotAngleY, out rotY);
//Microsoft.Xna.Framework.Matrix.CreateRotationZ((float)rotAngleZ, out rotZ);
//Vector3 tempV = new Vector3((float)worldX, (float)worldY, (float)worldZ);
//tempV = Vector3.Transform(tempV, rotX);
//tempV = Vector3.Transform(tempV, rotY);
//tempV = Vector3.Transform(tempV, rotZ);
//worldX = tempV.X;
//worldY = tempV.Y;
//worldZ = tempV.Z;
Vector3 WorldCoodrinates = new Vector3(worldX, worldY, worldZ);
return(WorldCoodrinates);
}
public RegisteredCamera(float FocalLength, float k1, float k2, Matrix RotationMatrix, Vector3 TranslationVector, int ImageHeight, int ImageWidth)
{
this.FocalLength = FocalLength;
this.k1 = k1;
this.k2 = k2;
this.RotationMatrix = RotationMatrix;
this.TranslationVector = TranslationVector;
this.ImageHeight = ImageHeight;
this.ImageWidth = ImageWidth;
Double[,] aTranslationMatrix = new Double[3, 1];
aTranslationMatrix[0, 0] = TranslationVector.X;
aTranslationMatrix[1, 0] = TranslationVector.Y;
aTranslationMatrix[2, 0] = TranslationVector.Z;
Double[,] aRotationMatrix = new Double[3, 3];
aRotationMatrix[0, 0] = RotationMatrix.M11;
aRotationMatrix[0, 1] = RotationMatrix.M12;
aRotationMatrix[0, 2] = RotationMatrix.M13;
aRotationMatrix[1, 0] = RotationMatrix.M21;
aRotationMatrix[1, 1] = RotationMatrix.M22;
aRotationMatrix[1, 2] = RotationMatrix.M23;
aRotationMatrix[2, 0] = RotationMatrix.M31;
aRotationMatrix[2, 1] = RotationMatrix.M32;
aRotationMatrix[2, 2] = RotationMatrix.M33;
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseRotationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(aRotationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseTranslationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(aTranslationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix densePositionVector = (-(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Transpose()) * denseTranslationMatrix;
PositionVector = new Vector3((float)densePositionVector[0, 0], (float)densePositionVector[1, 0], (float)densePositionVector[2, 0]);
//denseTranslationMatrix = -(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Transpose() * denseTranslationMatrix;
//TranslationVector.X = (float)denseTranslationMatrix[0, 0];
//TranslationVector.Y = (float)denseTranslationMatrix[1, 0];
//TranslationVector.Z = (float)denseTranslationMatrix[2, 0];
}
public static void ReceiveMessage()
{
while (true)
{
try
{
byte[] data = new byte[64];
StringBuilder builder = new StringBuilder();
int bytes = 0;
do
{
bytes = stream.Read(data, 0, data.Length);
builder.Append(Encoding.Unicode.GetString(data, 0, bytes));
}while (stream.DataAvailable);
string message = builder.ToString();
serverMessage sm = JsonConvert.DeserializeObject <serverMessage>(message);
switch (sm.type)
{
case "messageerror":
Console.WriteLine("Ошибка, сообщение невозможно отправить: " + sm.data);
break;
case "usersonline":
string[] usersList = sm.data.Split('|');
Console.WriteLine("Список пользователей онлайн:");
for (int i = 1; i <= usersList.Length; i++)
{
Console.WriteLine(i.ToString() + ". " + usersList[i - 1]);
}
break;
case "rejectconnection":
Console.WriteLine("Невозможно установить соединение: " + sm.data);
break;
case "connections":
string[] onlineConnections = sm.data.Split('|');
if (onlineConnections[0] == "")
{
Console.WriteLine("Подключения отсутствуют");
break;
}
Console.WriteLine("Текущие подключения:");
for (int i = 0; i < onlineConnections.Length; i++)
{
Console.WriteLine(i + ". " + onlineConnections[i]);
}
break;
case "alreadydisconnected":
Console.WriteLine("Пользователь " + sm.data + " не подключен");
break;
case "removeconnection":
Console.WriteLine("Пользователь " + sm.data + " отключен");
break;
case "connectionremoved":
Console.WriteLine("Пользователь " + sm.data + " отключился");
break;
case "acceptconnection":
Console.WriteLine("Пользователь " + sm.data + " устанавливает соединение...");
clientMessage cm = new clientMessage()
{
type = "accept", data = sm.data
};
string json = JsonConvert.SerializeObject(cm);
Console.WriteLine("JSON : " + json);
sendMessage(json);
break;
case "accept":
string[] keys = sm.data.Split('|');
string mySecretKey = keys[1];
string companionPublicKey = keys[2];
string[] mscdataString = mySecretKey.Split(' ');
string[] cpkdataString = companionPublicKey.Split(' ');
double[] mscdata = new double[mscdataString.Length];
double[] cpkdata = new double[cpkdataString.Length];
for (int i = 0; i < mscdataString.Length; i++)
{
mscdata[i] = Int32.Parse(mscdataString[i]);
cpkdata[i] = Int32.Parse(cpkdataString[i]);
}
Matrix <double> msc = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(mscdata.Length, 1, mscdata);
Matrix <double> cpk = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(cpkdata.Length, 1, cpkdata);
//Console.WriteLine("MY SEC: " + msc.ToString());
//Console.WriteLine("IT PUB: " + cpk.ToString());
msc = msc.Transpose();
Matrix <double> sKey = msc.Multiply(cpk);
sKey = sKey.Modulus(50);
commonSecretKey = (int)sKey[0, 0];
//Console.WriteLine("RES: " + Convert.ToString(commonSecretKey));
string Digits = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";
byte[] data123 = BitConverter.GetBytes(commonSecretKey);
BigInteger intData = 0;
for (int i = 0; i < data123.Length; i++)
{
intData = intData * 256 + data123[i];
}
string result = "";
while (intData > 0)
{
int remainder = (int)(intData % 58);
intData /= 58;
result = Digits[remainder] + result;
}
for (int i = 0; i < data123.Length && data123[i] == 0; i++)
{
result = '1' + result;
}
csk = result;
Console.WriteLine("СГЕНЕРИРОВАН СЕКРЕТНЫЙ КЛЮЧ: " + csk);
Console.WriteLine("Установлена сессия с пользователем " + sm.name);
RC4Cipher newCipher = new RC4Cipher(csk, 19);
session newSession = new session()
{
name = sm.name, cipher = newCipher
};
addSession(newSession);
break;
default:
string fp = sm.type.Split('|')[0];
string sp = sm.type.Split('|')[1];
session rs = GetSession(sp);
if (fp == "message")
{
Console.WriteLine(sp + "(шифрованное): " + sm.data);
string decodemessage = rs.cipher.encode(sm.data);
Console.WriteLine(sp + "(дешифрованное): " + decodemessage);
}
break;
}
}
catch (Exception e)
{
Console.WriteLine("Подключение прервано! " + e);
Console.ReadLine();
Disconnect();
}
}
}
public Vector3 ImageToWorld(double u, double v, double w, bool transposeRot, bool negateRot, bool invertRot, bool invertZ, float rotAngleX, float rotAngleY, float rotAngleZ)
{
double xprime = (u - ((float)(ImageWidth / 2))) / FocalLength;
double yprime = (v - ((float)(ImageHeight / 2))) / FocalLength;
//double xprime = (u) / FocalLength;
//double yprime = (v) / FocalLength;
if (invertZ)
{
w = -w;
}
double x = xprime * w;
double y = yprime * w;
double z = w;
Double[,] homoXYZ = new Double[3, 1];
homoXYZ[0, 0] = x;
homoXYZ[1, 0] = y;
homoXYZ[2, 0] = z;
Double[,] TranslationMatrix = new Double[3, 1];
TranslationMatrix[0, 0] = PositionVector.X;
TranslationMatrix[1, 0] = PositionVector.Y;
TranslationMatrix[2, 0] = PositionVector.Z;
Double[,] aRotationMatrix = new Double[3, 3];
aRotationMatrix[0, 0] = RotationMatrix.M11;
aRotationMatrix[0, 1] = RotationMatrix.M12;
aRotationMatrix[0, 2] = RotationMatrix.M13;
aRotationMatrix[1, 0] = RotationMatrix.M21;
aRotationMatrix[1, 1] = RotationMatrix.M22;
aRotationMatrix[1, 2] = RotationMatrix.M23;
aRotationMatrix[2, 0] = RotationMatrix.M31;
aRotationMatrix[2, 1] = RotationMatrix.M32;
aRotationMatrix[2, 2] = RotationMatrix.M33;
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseHomoXYZ = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(homoXYZ);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseTranslationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(TranslationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseRotationMatrix = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(aRotationMatrix);
if (transposeRot)
{
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Transpose();
}
if (invertRot)
{
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Inverse();
}
denseRotationMatrix = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)denseRotationMatrix.Inverse();
if (negateRot)
{
denseRotationMatrix = -denseRotationMatrix;
}
//Double[,] TranslationMatrix = new Double[3, 1];
//TranslationMatrix[0, 0] = TranslationVector.X;
//TranslationMatrix[1, 0] = TranslationVector.Y;
//TranslationMatrix[2, 0] = TranslationVector.Z;
//denseRotationMatrix = denseRotationMatrix.Transpose() *
//MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseWorldCoordinates = InverseRotationMatrix * (denseHomoXYZ - TranslationMatrix);
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix denseWorldCoordinates = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)(denseRotationMatrix) * (denseHomoXYZ - denseTranslationMatrix);
float worldX = (float)denseWorldCoordinates[0, 0];
float worldY = (float)denseWorldCoordinates[1, 0];
float worldZ = (float)denseWorldCoordinates[2, 0];
//Microsoft.Xna.Framework.Matrix rotX;
//Microsoft.Xna.Framework.Matrix rotY;
//Microsoft.Xna.Framework.Matrix rotZ;
//Vector3 origin = new Vector3(0, 0, 0);
//Microsoft.Xna.Framework.Matrix.CreateRotationX((float)rotAngleX, out rotX);
//Microsoft.Xna.Framework.Matrix.CreateRotationY((float)rotAngleY, out rotY);
//Microsoft.Xna.Framework.Matrix.CreateRotationZ((float)rotAngleZ, out rotZ);
//Vector3 tempV = new Vector3((float)worldX, (float)worldY, (float)worldZ);
//tempV = Vector3.Transform(tempV, rotX);
//tempV = Vector3.Transform(tempV, rotY);
//tempV = Vector3.Transform(tempV, rotZ);
//worldX = tempV.X;
//worldY = tempV.Y;
//worldZ = tempV.Z;
Vector3 WorldCoodrinates = new Vector3(worldX, worldY, worldZ);
return WorldCoodrinates;
}
