#include "stdafx.h"
#include "math.h"
#include "ADCSArithmetic.h"
/*
** 描述：
** 向量的叉乘
*/
Vector VectorCross(const Vector In_A, const Vector In_B)
{
Vector Out_C = { 0 };
Out_C.Value[0] = In_A.Value[1] * In_B.Value[2] - In_A.Value[2] * In_B.Value[1];
Out_C.Value[1] = In_A.Value[2] * In_B.Value[0] - In_A.Value[0] * In_B.Value[2];
Out_C.Value[2] = In_A.Value[0] * In_B.Value[1] - In_A.Value[1] * In_B.Value[0];
return Out_C;
}
/*
** 描述：
** 向量的点乘
*/
double VectorDot(const Vector In_A, const Vector In_B)
{
return(In_A.Value[0] * In_B.Value[0] + In_A.Value[1] * In_B.Value[1] + In_A.Value[2] * In_B.Value[2]);
}
/*
** 描述：
** 向量乘标量
*/
Vector VectorXScalar(double ScalarValue, const Vector In_Value)
{
Vector Out_Value = { 0 };
Out_Value.Value[0] = ScalarValue*In_Value.Value[0];
Out_Value.Value[1] = ScalarValue*In_Value.Value[1];
Out_Value.Value[2] = ScalarValue*In_Value.Value[2];
return Out_Value;
}
/*
** 描述：
** 向量的加法
*/
Vector VectorAddition(Vector In_A, const Vector In_B)
{
Vector Out_C = { 0 };
Out_C.Value[0] = In_A.Value[0] + In_B.Value[0];
Out_C.Value[1] = In_A.Value[1] + In_B.Value[1];
Out_C.Value[2] = In_A.Value[2] + In_B.Value[2];
return Out_C;
}
/*
** 描述：
** 向量的减法
*/
Vector VectorSubtraction(Vector In_A, const Vector In_B)
{
Vector Out_C = { 0 };
Out_C.Value[0] = In_A.Value[0] - In_B.Value[0];
Out_C.Value[1] = In_A.Value[1] - In_B.Value[1];
Out_C.Value[2] = In_A.Value[2] - In_B.Value[2];
return Out_C;
}
/*
** 描述：
** 横向量(1*3)转置成竖向量(3*1)
** 返回：
** 竖向量(3*1)
*/
TVector VectorTranspose(Vector *Vector_Value)
{
TVector TVector_Out = { 0 };
memcpy((void*)&TVector_Out, (void*)Vector_Value, sizeof(TVector));
return TVector_Out;
}
/*
** 描述：
** 竖向量(3*1)转置乘横向量(1*3)
** 返回：
** 横向量(1*3)
*/
Vector TVectorTranspose(TVector *TVector_Value)
{
Vector Vector_Out = { 0 };
memcpy((void*)&Vector_Out, (void*)TVector_Value, sizeof(TVector));
return Vector_Out;
}
/*
** 描述：
** 将向量中的量限制在[Min,Max]范围内
** 返回：
** 横向量(1*3)
*/
Vector VectorLimit(Vector Vector_Value, double Min, double Max)
{
Vector Vector_Out = Vector_Value;
int i = 0;
for (i = 0; i < sizeof(Vector_Out.Value) / sizeof(double); i++)
{
if (Vector_Out.Value[i] < Min)
{
Vector_Out.Value[i] = Min;
}
else if (Vector_Out.Value[i] > Max)
{
Vector_Out.Value[i] = Max;
}
}
return Vector_Out;
}
/*
** 描述：
** 3*3矩阵乘以竖向量(3*1)
** 返回：
** 竖向量(3*1)
*/
TVector MatrixXTVector(Matrix Matrix_Value, TVector TVector_Value)
{
TVector TVector_Out = { 0 };
TVector_Out.Value[0] =
Matrix_Value.Value[0][0] * TVector_Value.Value[0] +
Matrix_Value.Value[0][1] * TVector_Value.Value[1] +
Matrix_Value.Value[0][2] * TVector_Value.Value[2];
TVector_Out.Value[1] =
Matrix_Value.Value[1][0] * TVector_Value.Value[0] +
Matrix_Value.Value[1][1] * TVector_Value.Value[1] +
Matrix_Value.Value[1][2] * TVector_Value.Value[2];
TVector_Out.Value[2] =
Matrix_Value.Value[2][0] * TVector_Value.Value[0] +
Matrix_Value.Value[2][1] * TVector_Value.Value[1] +
Matrix_Value.Value[2][2] * TVector_Value.Value[2];
return TVector_Out;
}
/*
** 描述：
** 四元数乘法
** 返回：
** 四元数
*/
Quaternion MatrixXTVector(Quaternion Quaternion_A, Quaternion Quaternion_B)
{
Quaternion Quaternion_Out = { 0 };
Quaternion_Out.Value[0] =
Quaternion_A.Value[3] * Quaternion_B.Value[0] +
Quaternion_A.Value[2] * Quaternion_B.Value[1] -
Quaternion_A.Value[1] * Quaternion_B.Value[2] +
Quaternion_A.Value[0] * Quaternion_B.Value[3];
Quaternion_Out.Value[1] =
-Quaternion_A.Value[2] * Quaternion_B.Value[0] +
Quaternion_A.Value[3] * Quaternion_B.Value[1] +
Quaternion_A.Value[0] * Quaternion_B.Value[2] +
Quaternion_A.Value[1] * Quaternion_B.Value[3];
Quaternion_Out.Value[2] =
Quaternion_A.Value[1] * Quaternion_B.Value[0] -
Quaternion_A.Value[0] * Quaternion_B.Value[1] +
Quaternion_A.Value[3] * Quaternion_B.Value[2] +
Quaternion_A.Value[2] * Quaternion_B.Value[3];
Quaternion_Out.Value[3] =
-Quaternion_A.Value[0] * Quaternion_B.Value[0] -
Quaternion_A.Value[1] * Quaternion_B.Value[1] -
Quaternion_A.Value[2] * Quaternion_B.Value[2] +
Quaternion_A.Value[3] * Quaternion_B.Value[3];
return Quaternion_Out;
}
//
///*
//** 描述：
//** 判断两个double类型数据是否相等
//*/
//bool DoubleEquals(double a, double b)
//{
// if (fabs(a - b) < 1e-6)
// {
// return true;
// }
// else
// {
// return false;
// }
//}
评论0