///<summary>
/// Receive a rotation matrix of type Matrix4
/// Return a Vector3Float containing the rotation around the 3 axis
///</summary>
public static Vecto3Float computeEulerFromMatrixXYZ(Matrix3x3 m)
{
Vecto3Float sol = new Vecto3Float();
if (m.matrix[0, 2] < 1f)
{
if (m.matrix[0, 2] > -1f)
{
sol.X = (float)Math.Atan2(-m.matrix[1, 2], m.matrix[2, 2]);
sol.Y = (float)Math.Asin(-(m.matrix[0, 2]));
sol.Z = (float)Math.Atan2(-m.matrix[0, 1], m.matrix[0, 0]);
}
else
{
sol.X = -(float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]);
sol.Y = -(float)(Math.PI) / 2;
sol.Z = 0f;
}
}
else
{
sol.X = (float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]);
sol.Y = (float)(Math.PI) / 2;
sol.Z = 0f;
}
return sol;
}
///<summary>
/// Receive three skeleton point (3D position of a joint).
/// Compute two vector, return the angle between them.
/// If something is wrong return 0
///</summary>
public static float getAngle(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c, bool sign)
{
Vecto3Float[] vector = new Vecto3Float[2];
//vector[0] = new Vecto3Float(b.X - c.X, b.Y - c.Y, b.Z - c.Z);
vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, c.Z - b.Z);
vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
float v0_magnitude = vector[0].magnitude();
float v1_magnitude = vector[1].magnitude();
if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
{
vector[0].normalize();
vector[1].normalize();
float sign_f = sign ? -1f : 1f;
double x = (double)vector[0].cross(vector[1]).magnitude();
double y = (double)vector[0].dot(vector[1]);
float theta = (float)Math.Atan2(sign_f * x, sign_f * y);
return theta;// -theta;
}
else
{
return 0.0f;
}
}
public static float getAngleXY(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c)
{
Vecto3Float[] vector = new Vecto3Float[2];
vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, 0);
vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, 0);
float v0_magnitude = vector[0].magnitude();
float v1_magnitude = vector[1].magnitude();
if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
{
vector[0].normalize();
vector[1].normalize();
double x = (double)vector[0].cross(vector[1]).magnitude();
double y = (double)vector[0].dot(vector[1]);
float theta = (float)Math.Atan2(x, y);
return(-theta);
}
else
{
return(0.0f);
}
}
///<summary>
/// Receive a rotation matrix of type Matrix4
/// Return a Vector3Float containing the rotation around the 3 axis
///</summary>
public static Vecto3Float computeEulerFromMatrixXYZ(Matrix3x3 m)
{
Vecto3Float sol = new Vecto3Float();
if (m.matrix[0, 2] < 1f)
{
if (m.matrix[0, 2] > -1f)
{
sol.X = (float)Math.Atan2(-m.matrix[1, 2], m.matrix[2, 2]);
sol.Y = (float)Math.Asin(-(m.matrix[0, 2]));
sol.Z = (float)Math.Atan2(-m.matrix[0, 1], m.matrix[0, 0]);
}
else
{
sol.X = -(float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]);
sol.Y = -(float)(Math.PI) / 2;
sol.Z = 0f;
}
}
else
{
sol.X = (float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]);
sol.Y = (float)(Math.PI) / 2;
sol.Z = 0f;
}
return(sol);
}
public static float getAngleZY(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c)
{
Vecto3Float[] vector = new Vecto3Float[2];
//vector[0] = new Vecto3Float(0, b.Y - c.Y, b.Z - c.Z);
vector[0] = new Vecto3Float(0, c.Y - b.Y, c.Z - b.Z);
vector[1] = new Vecto3Float(0, a.Y - b.Y, a.Z - b.Z);
float v0_magnitude = vector[0].magnitude();
float v1_magnitude = vector[1].magnitude();
if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
{
vector[0].normalize();
vector[1].normalize();
double x = (double)vector[0].cross(vector[1]).magnitude();
double y = (double)vector[0].dot(vector[1]);
float theta = (float)Math.Atan2(x, y);
float theta2 = (float)Math.Atan2(-x, y);
float theta3 = (float)Math.Atan2(x, -y);
float theta4 = (float)Math.Atan2(-x, -y);
return(-theta);
}
else
{
return(0.0f);
}
}
///<summary>
/// Compute some angle using rotation matrix
///</summary>
private void angleMatrix(Skeleton skel)
{
//ELBOWYAW
//Left
Matrix3x3 m_l = new Matrix3x3(skel.BoneOrientations[JointType.ElbowLeft].HierarchicalRotation.Matrix);
Vecto3Float v_l = Kinematic.computeEulerFromMatrixXYZ(m_l);
this.jointAngles[NAOConversion.LElbowYaw] = this.jointAngles[NAOConversion.LElbowYaw]
+ NAOConversion.convertAngle(NAOConversion.LElbowYaw, v_l.Z) / this.avg;
//Right
Matrix3x3 m_r = new Matrix3x3(skel.BoneOrientations[JointType.ElbowRight].HierarchicalRotation.Matrix);
Vecto3Float v_r = Kinematic.computeEulerFromMatrixXYZ(m_r);
this.jointAngles[NAOConversion.RElbowYaw] = this.jointAngles[NAOConversion.RElbowYaw]
+ NAOConversion.convertAngle(NAOConversion.RElbowYaw, v_r.Z) / this.avg;
//HEAD
Matrix3x3 m_head = new Matrix3x3(skel.BoneOrientations[JointType.Head].HierarchicalRotation.Matrix);
Vecto3Float v_head = Kinematic.computeEulerFromMatrixXYZ(m_head);
//Yaw
this.jointAngles[NAOConversion.HeadYaw] = this.jointAngles[NAOConversion.HeadYaw]
+ NAOConversion.convertAngle(NAOConversion.HeadYaw, v_head.Z) / this.avg;
//Pitch
this.jointAngles[NAOConversion.HeadPitch] = this.jointAngles[NAOConversion.HeadPitch]
+ NAOConversion.convertAngle(NAOConversion.HeadPitch, v_head.X) / this.avg;
}
///<summary>
/// Receive three skeleton point (3D position of a joint).
/// Compute two vector, return the angle between them.
/// If something is wrong return 0
///</summary>
public static float getAngle(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c, bool sign)
{
Vecto3Float[] vector = new Vecto3Float[2];
//vector[0] = new Vecto3Float(b.X - c.X, b.Y - c.Y, b.Z - c.Z);
vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, c.Z - b.Z);
vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
float v0_magnitude = vector[0].magnitude();
float v1_magnitude = vector[1].magnitude();
if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
{
vector[0].normalize();
vector[1].normalize();
float sign_f = sign ? -1f : 1f;
double x = (double)vector[0].cross(vector[1]).magnitude();
double y = (double)vector[0].dot(vector[1]);
float theta = (float)Math.Atan2(sign_f * x, sign_f * y);
return(theta);// -theta;
}
else
{
return(0.0f);
}
}
public Vecto3Float cross(Vecto3Float vec)
{
Vecto3Float res = new Vecto3Float();
res.X = this.Y * vec.Z - vec.Y * this.Z;
res.Y = this.Z * vec.X - vec.Z * this.X;
res.Z = this.X * vec.Y - vec.Z * this.Y;
return(res);
}
public Vecto3Float cross(Vecto3Float vec)
{
Vecto3Float res = new Vecto3Float();
res.X = this.Y * vec.Z - vec.Y * this.Z;
res.Y = this.Z * vec.X - vec.Z * this.X;
res.Z = this.X * vec.Y - vec.Z * this.Y;
return res;
}
///<summary>
/// Compute some angle using rotation matrix
///</summary>
private void angleMatrix(Skeleton skel)
{
//HEAD
Matrix3x3 m_head = new Matrix3x3(skel.BoneOrientations[JointType.Head].HierarchicalRotation.Matrix);
Vecto3Float v_head = Kinematic.computeEulerFromMatrixXYZ(m_head);
//Yaw
this.jointAngles[NAOConversion.HeadYaw] = this.jointAngles[NAOConversion.HeadYaw]
+ NAOConversion.convertAngle(NAOConversion.HeadYaw, v_head.Z) / this.avg;
//Pitch
this.jointAngles[NAOConversion.HeadPitch] = this.jointAngles[NAOConversion.HeadPitch]
+ NAOConversion.convertAngle(NAOConversion.HeadPitch, v_head.X) / this.avg;
}
public static float getAngleNew(Vecto3Float vect1, Vecto3Float vect2)
{
vect1.normalize();
vect2.normalize();
if (vect1.magnitude() != 0.0 && vect2.magnitude() != 0.0)
{
double x = (double)vect1.cross(vect2).magnitude();
double y = (double)vect1.dot(vect2);
return atan2(x, y);
}
else
{
return 0f;
}
}
public static float getAngleNew(Vecto3Float vect1, Vecto3Float vect2)
{
vect1.normalize();
vect2.normalize();
if (vect1.magnitude() != 0.0 && vect2.magnitude() != 0.0)
{
double x = (double)vect1.cross(vect2).magnitude();
double y = (double)vect1.dot(vect2);
return(atan2(x, y));
}
else
{
return(0f);
}
}
///<summary>
/// Compute angle between the joint according to the position of the joint
///</summary>
private void angleVector()
{
SkeletonPoint shoulderLeft = this.avgPoint[JointType.ShoulderLeft];
SkeletonPoint shoulderCenter = this.avgPoint[JointType.ShoulderCenter];
SkeletonPoint shoulderRight = this.avgPoint[JointType.ShoulderRight];
SkeletonPoint elbowLeft = this.avgPoint[JointType.ElbowLeft];
SkeletonPoint elbowRight = this.avgPoint[JointType.ElbowRight];
SkeletonPoint wristLeft = this.avgPoint[JointType.WristLeft];
SkeletonPoint wristRight = this.avgPoint[JointType.WristRight];
SkeletonPoint hipLeft = this.avgPoint[JointType.HipLeft];
SkeletonPoint hipCenter = this.avgPoint[JointType.HipCenter];
SkeletonPoint hipRight = this.avgPoint[JointType.HipRight];
Vecto3Float upperArmLeft = new Vecto3Float(shoulderLeft.X - elbowLeft.X,
shoulderLeft.Y - elbowLeft.Y,
shoulderLeft.Z - elbowLeft.Z);
Vecto3Float lowerArmLeft = new Vecto3Float(wristLeft.X - elbowLeft.X,
wristLeft.Y - elbowLeft.Y,
wristLeft.Z - elbowLeft.Z);
Vecto3Float upperArmRight = new Vecto3Float(shoulderRight.X - elbowRight.X,
shoulderRight.Y - elbowRight.Y,
shoulderRight.Z - elbowRight.Z);
Vecto3Float lowerArmRight = new Vecto3Float(wristRight.X - elbowRight.X,
wristRight.Y - elbowRight.Y,
wristRight.Z - elbowRight.Z);
Vecto3Float shoulderLToHipLeft = new Vecto3Float(shoulderLeft.X - hipLeft.X,
shoulderLeft.Y - hipLeft.Y,
shoulderLeft.Z - hipLeft.Z);
Vecto3Float shoulderRToHipRight = new Vecto3Float(shoulderRight.X - hipRight.X,
shoulderRight.Y - hipRight.Y,
shoulderRight.Z - hipRight.Z);
Vecto3Float shoulderLToShoulderC = new Vecto3Float(shoulderLeft.X - shoulderCenter.X,
shoulderLeft.Y - shoulderCenter.Y,
shoulderLeft.Z - shoulderCenter.Z);
Vecto3Float shoulderRToShoulderC = new Vecto3Float(shoulderRight.X - shoulderCenter.X,
shoulderRight.Y - shoulderCenter.Y,
shoulderRight.Z - shoulderCenter.Z);
if (hipLeft.Z + 0.15 > elbowLeft.Z) //if the elbow is in front of the hip
{
//Shoulder Roll
//Left
if (Math.Abs(shoulderLeft.Z - elbowLeft.Z) < 0.1 &&
Math.Abs(shoulderLeft.X - elbowLeft.X) < 0.1)
{
this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll,
Kinematic.getAngleNew(upperArmLeft.projectionOntoXY(),
shoulderLToHipLeft.projectionOntoXY()));
}
else
{
this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll,
Kinematic.getAngleNew(upperArmLeft.projectionOntoZX(),
shoulderLToHipLeft.projectionOntoZX()));
}
//Shoulder Pitch
//Left
this.jointAngles[NAOConversion.LShoulderPitch] = NAOConversion.convertAngle(NAOConversion.LShoulderPitch,
Kinematic.getAngleNew(upperArmLeft.projectionOntoZY(),
shoulderLToHipLeft.projectionOntoZY()));
}
if (hipRight.Z + 0.15 > elbowRight.Z)//if the elbow is in front of the hip
{
//Shoulder Roll
//Right
if (Math.Abs(shoulderRight.Z - elbowRight.Z) < 0.1 &&
Math.Abs(shoulderRight.X - elbowRight.X) < 0.1)
{
this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll,
Kinematic.getAngleNew(upperArmRight.projectionOntoXY(),
shoulderRToHipRight.projectionOntoXY()));
}
else
{
this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll,
Kinematic.getAngleNew(upperArmRight.projectionOntoZX(),
shoulderRToHipRight.projectionOntoZX()));
}
//Shoulder Pitch
//Right
this.jointAngles[NAOConversion.RShoulderPitch] = NAOConversion.convertAngle(NAOConversion.RShoulderPitch,
Kinematic.getAngleNew(shoulderRToHipRight.projectionOntoZY(),
upperArmRight.projectionOntoZY()));
}
//Elbow Roll
//Left
this.jointAngles[NAOConversion.LElbowRoll] = NAOConversion.convertAngle(NAOConversion.LElbowRoll,
Kinematic.getAngleNew(upperArmLeft, lowerArmLeft));
//Right
this.jointAngles[NAOConversion.RElbowRoll] = NAOConversion.convertAngle(NAOConversion.RElbowRoll,
Kinematic.getAngleNew(upperArmRight, lowerArmRight));
//Elbow Yaw
////Left
Vecto3Float shoulderLTohipCenter = new Vecto3Float(shoulderLeft.X - hipCenter.X,
shoulderLeft.Y - hipCenter.Y,
shoulderLeft.Z - hipCenter.Z);
Vecto3Float lCrossCenterArm = upperArmLeft.cross(shoulderLTohipCenter);
Vecto3Float lCrossArms = upperArmLeft.cross(lowerArmLeft);
float lElbowYaw = Kinematic.getAngleNew(lCrossCenterArm, lCrossArms);
this.jointAngles[NAOConversion.LElbowYaw] = NAOConversion.convertAngle(NAOConversion.LElbowYaw, lElbowYaw);
//Right
Vecto3Float shoulderRTohipCenter = new Vecto3Float(shoulderRight.X - hipCenter.X,
shoulderRight.Y - hipCenter.Y,
shoulderRight.Z - hipCenter.Z);
Vecto3Float rCrossCenterArm = upperArmRight.cross(shoulderRTohipCenter);
Vecto3Float rCrossArms = upperArmRight.cross(lowerArmRight);
float rElbowYaw = Kinematic.getAngleNew(rCrossCenterArm, rCrossArms);
this.jointAngles[NAOConversion.RElbowYaw] = NAOConversion.convertAngle(NAOConversion.RElbowYaw, rElbowYaw);
}
///<summary>
/// Compute angle between the joint according to the position of the joint
///</summary>
private void angleVector()
{
SkeletonPoint shoulderLeft = this.avgPoint[JointType.ShoulderLeft];
SkeletonPoint shoulderCenter = this.avgPoint[JointType.ShoulderCenter];
SkeletonPoint shoulderRight = this.avgPoint[JointType.ShoulderRight];
SkeletonPoint elbowLeft = this.avgPoint[JointType.ElbowLeft];
SkeletonPoint elbowRight = this.avgPoint[JointType.ElbowRight];
SkeletonPoint wristLeft = this.avgPoint[JointType.WristLeft];
SkeletonPoint wristRight = this.avgPoint[JointType.WristRight];
SkeletonPoint hipLeft = this.avgPoint[JointType.HipLeft];
SkeletonPoint hipCenter = this.avgPoint[JointType.HipCenter];
SkeletonPoint hipRight = this.avgPoint[JointType.HipRight];
Vecto3Float upperArmLeft = new Vecto3Float(shoulderLeft.X - elbowLeft.X,
shoulderLeft.Y - elbowLeft.Y,
shoulderLeft.Z - elbowLeft.Z);
Vecto3Float lowerArmLeft = new Vecto3Float(wristLeft.X - elbowLeft.X,
wristLeft.Y - elbowLeft.Y,
wristLeft.Z - elbowLeft.Z);
Vecto3Float upperArmRight = new Vecto3Float(shoulderRight.X - elbowRight.X,
shoulderRight.Y - elbowRight.Y,
shoulderRight.Z - elbowRight.Z);
Vecto3Float lowerArmRight = new Vecto3Float(wristRight.X - elbowRight.X,
wristRight.Y - elbowRight.Y,
wristRight.Z - elbowRight.Z);
Vecto3Float shoulderLToHipLeft = new Vecto3Float(shoulderLeft.X - hipLeft.X,
shoulderLeft.Y - hipLeft.Y,
shoulderLeft.Z - hipLeft.Z);
Vecto3Float shoulderRToHipRight = new Vecto3Float(shoulderRight.X - hipRight.X,
shoulderRight.Y - hipRight.Y,
shoulderRight.Z - hipRight.Z);
Vecto3Float shoulderLToShoulderC = new Vecto3Float(shoulderLeft.X - shoulderCenter.X,
shoulderLeft.Y - shoulderCenter.Y,
shoulderLeft.Z - shoulderCenter.Z);
Vecto3Float shoulderRToShoulderC = new Vecto3Float(shoulderRight.X - shoulderCenter.X,
shoulderRight.Y - shoulderCenter.Y,
shoulderRight.Z - shoulderCenter.Z);
if (hipLeft.Z + 0.15 > elbowLeft.Z) //if the elbow is in front of the hip
{
//Shoulder Roll
//Left
if (Math.Abs(shoulderLeft.Z - elbowLeft.Z) < 0.1 &&
Math.Abs(shoulderLeft.X - elbowLeft.X) < 0.1)
{
this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll,
Kinematic.getAngleNew(upperArmLeft.projectionOntoXY(),
shoulderLToHipLeft.projectionOntoXY()));
}
else
{
this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll,
Kinematic.getAngleNew(upperArmLeft.projectionOntoZX(),
shoulderLToHipLeft.projectionOntoZX()));
}
//Shoulder Pitch
//Left
this.jointAngles[NAOConversion.LShoulderPitch] = NAOConversion.convertAngle(NAOConversion.LShoulderPitch,
Kinematic.getAngleNew(upperArmLeft.projectionOntoZY(),
shoulderLToHipLeft.projectionOntoZY()));
}
if (hipRight.Z + 0.15 > elbowRight.Z)//if the elbow is in front of the hip
{
//Shoulder Roll
//Right
if (Math.Abs(shoulderRight.Z - elbowRight.Z) < 0.1 &&
Math.Abs(shoulderRight.X - elbowRight.X) < 0.1)
{
this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll,
Kinematic.getAngleNew(upperArmRight.projectionOntoXY(),
shoulderRToHipRight.projectionOntoXY()));
}
else
{
this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll,
Kinematic.getAngleNew(upperArmRight.projectionOntoZX(),
shoulderRToHipRight.projectionOntoZX()));
}
//Shoulder Pitch
//Right
this.jointAngles[NAOConversion.RShoulderPitch] = NAOConversion.convertAngle(NAOConversion.RShoulderPitch,
Kinematic.getAngleNew(shoulderRToHipRight.projectionOntoZY(),
upperArmRight.projectionOntoZY()));
}
//Elbow Roll
//Left
this.jointAngles[NAOConversion.LElbowRoll] = NAOConversion.convertAngle(NAOConversion.LElbowRoll,
Kinematic.getAngleNew(upperArmLeft, lowerArmLeft));
//Right
this.jointAngles[NAOConversion.RElbowRoll] = NAOConversion.convertAngle(NAOConversion.RElbowRoll,
Kinematic.getAngleNew(upperArmRight, lowerArmRight));
//Elbow Yaw
////Left
Vecto3Float shoulderLTohipCenter = new Vecto3Float(shoulderLeft.X - hipCenter.X,
shoulderLeft.Y - hipCenter.Y,
shoulderLeft.Z - hipCenter.Z);
Vecto3Float lCrossCenterArm = upperArmLeft.cross(shoulderLTohipCenter);
Vecto3Float lCrossArms = upperArmLeft.cross(lowerArmLeft);
float lElbowYaw = Kinematic.getAngleNew(lCrossCenterArm, lCrossArms);
this.jointAngles[NAOConversion.LElbowYaw] = NAOConversion.convertAngle(NAOConversion.LElbowYaw, lElbowYaw);
//Right
Vecto3Float shoulderRTohipCenter = new Vecto3Float(shoulderRight.X - hipCenter.X,
shoulderRight.Y - hipCenter.Y,
shoulderRight.Z - hipCenter.Z);
Vecto3Float rCrossCenterArm = upperArmRight.cross(shoulderRTohipCenter);
Vecto3Float rCrossArms = upperArmRight.cross(lowerArmRight);
float rElbowYaw = Kinematic.getAngleNew(rCrossCenterArm, rCrossArms);
this.jointAngles[NAOConversion.RElbowYaw] = NAOConversion.convertAngle(NAOConversion.RElbowYaw, rElbowYaw);
}
public static float getAngleXY(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c)
{
Vecto3Float[] vector = new Vecto3Float[2];
vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, 0);
vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, 0);
float v0_magnitude = vector[0].magnitude();
float v1_magnitude = vector[1].magnitude();
if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
{
vector[0].normalize();
vector[1].normalize();
double x = (double)vector[0].cross(vector[1]).magnitude();
double y = (double)vector[0].dot(vector[1]);
float theta = (float)Math.Atan2(x, y);
return -theta;
}
else
{
return 0.0f;
}
}
public static float getAngleZY(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c)
{
Vecto3Float[] vector = new Vecto3Float[2];
//vector[0] = new Vecto3Float(0, b.Y - c.Y, b.Z - c.Z);
vector[0] = new Vecto3Float(0, c.Y - b.Y, c.Z - b.Z);
vector[1] = new Vecto3Float(0, a.Y - b.Y, a.Z - b.Z);
float v0_magnitude = vector[0].magnitude();
float v1_magnitude = vector[1].magnitude();
if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
{
vector[0].normalize();
vector[1].normalize();
double x = (double)vector[0].cross(vector[1]).magnitude();
double y = (double)vector[0].dot(vector[1]);
float theta = (float)Math.Atan2(x, y);
float theta2 = (float)Math.Atan2(-x, y);
float theta3 = (float)Math.Atan2(x, -y);
float theta4 = (float)Math.Atan2(-x, -y);
return -theta;
}
else
{
return 0.0f;
}
}
public float dot(Vecto3Float vect)
{
return this.x * vect.X + this.y * vect.Y + this.z * vect.Z;
}
public float dot(Vecto3Float vect)
{
return(this.x * vect.X + this.y * vect.Y + this.z * vect.Z);
}
