/*******************************************************************************
* Function Name : AES_expand_deckey
* Description : According to key computes the expanded key (expkey) for AES128
* decryption.
* Input : key: user key (128 bits / 16 bytes)
* Output : expkey: expanded key (320 bits / 40 bytes)
* Return : None
*******************************************************************************/
void AES_expand_deckey(ot_u32* key, ot_u32* expkey) {
#if (AES_USEHW)
platform_expand_deckey(key, expkey);
#elif (AES_USEFAST)
register ot_u32* local_pointer;// = expkey;
register int i; // = 0;
register ot_u32 copy0;
register ot_u32 copy1;
register ot_u32 copy2;
register ot_u32 copy3;
AES_expand_enckey(key,expkey);
local_pointer = expkey;
local_pointer[0] = key[0];
local_pointer[1] = key[1];
local_pointer[2] = key[2];
local_pointer[3] = key[3];
for (i = 1; i <10; i++) {
local_pointer += 4;
copy0 = local_pointer[0];
copy1 = local_pointer[1];
copy2 = local_pointer[2];
copy3 = local_pointer[3];
local_pointer[0] = dec_table[Sbox[byte0(copy0)]] ^
rot1(dec_table[Sbox[byte1(copy0)]]) ^
rot2(dec_table[Sbox[byte2(copy0)]]) ^
rot3(dec_table[Sbox[byte3(copy0)]]);
local_pointer[1] = dec_table[Sbox[byte0(copy1)]] ^
rot1(dec_table[Sbox[byte1(copy1)]]) ^
rot2(dec_table[Sbox[byte2(copy1)]]) ^
rot3(dec_table[Sbox[byte3(copy1)]]);
local_pointer[2] = dec_table[Sbox[byte0(copy2)]] ^
rot1(dec_table[Sbox[byte1(copy2)]]) ^
rot2(dec_table[Sbox[byte2(copy2)]]) ^
rot3(dec_table[Sbox[byte3(copy2)]]);
local_pointer[3] = dec_table[Sbox[byte0(copy3)]] ^
rot1(dec_table[Sbox[byte1(copy3)]]) ^
rot2(dec_table[Sbox[byte2(copy3)]]) ^
rot3(dec_table[Sbox[byte3(copy3)]]);
}
#elif (AES_USELITE == ENABLED)
AES_expand_enckey(key, expkey);
#endif
}
Vector3f Arm::F(VectorXf theta) {
Vector3f rot1(theta(0), theta(1), theta(2));
Vector3f rot2(theta(3), theta(4), theta(5));
Vector3f rot3(theta(6), theta(7), theta(8));
Vector3f rot4(theta(9), theta(10), theta(11));
Matrix4f R1; R1 = rodrigues(rot1);
Matrix4f R2; R2 = rodrigues(rot2);
Matrix4f R3; R3 = rodrigues(rot3);
Matrix4f R4; R4 = rodrigues(rot4);
Matrix4f T1; T1 = list_joints[0]->transformation();
Matrix4f T2; T2 = list_joints[1]->transformation();
Matrix4f T3; T3 = list_joints[2]->transformation();
Matrix4f T4; T4 = list_joints[3]->transformation();
Vector4f identity(0.0, 0.0, 0.0, 1.0);
Vector4f result;
result = R1 * T1 * R2 * T2 * R3 * T3 * R4 * T4 * identity;
Vector3f ret(result(0), result(1), result(2));
// cout << "Theta:" << endl << theta;
// cout << "ret: " << endl << ret << endl;=
return ret;
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
M113a_Chassis::M113a_Chassis(const std::string& name, bool fixed, ChassisCollisionType chassis_collision_type)
: ChRigidChassis(name, fixed) {
m_inertia.SetElement(0, 0, m_inertiaXX.x());
m_inertia.SetElement(1, 1, m_inertiaXX.y());
m_inertia.SetElement(2, 2, m_inertiaXX.z());
m_inertia.SetElement(0, 1, m_inertiaXY.x());
m_inertia.SetElement(0, 2, m_inertiaXY.y());
m_inertia.SetElement(1, 2, m_inertiaXY.z());
m_inertia.SetElement(1, 0, m_inertiaXY.x());
m_inertia.SetElement(2, 0, m_inertiaXY.y());
m_inertia.SetElement(2, 1, m_inertiaXY.z());
// Belly shape (all dimensions in cm)
// width: 170
// points in x-z transversal plane: (-417.0 -14.3), (4.1, -14.3), (21.4, 34.3)
// thickness: 20
double width = 1.70;
double Ax = -4.17;
double Az = -0.143;
double Bx = 0.041;
double Bz = -0.143;
double Cx = 0.214;
double Cz = 0.343;
double thickness = 0.2;
ChVector<> dims1((Bx - Ax), width, thickness);
ChVector<> loc1(0.5 * (Ax + Bx), 0.0, Az + 0.5 * thickness);
ChQuaternion<> rot1(1, 0, 0, 0);
BoxShape box1(loc1, rot1, dims1);
double alpha = std::atan2(Cz - Bz, Cx - Bx); // pitch angle of front box
ChVector<> dims2((Cx - Bx) / std::cos(alpha), width, thickness);
ChVector<> loc2(0.5 * (Bx + Cx) - 0.5 * thickness * std::sin(alpha), 0.0,
0.5 * (Bz + Cz) + 0.5 * thickness * std::cos(alpha));
ChQuaternion<> rot2 = Q_from_AngY(-alpha);
BoxShape box2(loc2, rot2, dims2);
m_has_primitives = true;
m_vis_boxes.push_back(box1);
m_vis_boxes.push_back(box2);
m_has_mesh = true;
m_vis_mesh_name = "Chassis_POV_geom";
m_vis_mesh_file = "M113/Chassis.obj";
m_has_collision = (chassis_collision_type != ChassisCollisionType::NONE);
switch (chassis_collision_type) {
case ChassisCollisionType::PRIMITIVES:
m_coll_boxes.push_back(box1);
m_coll_boxes.push_back(box2);
break;
case ChassisCollisionType::MESH:
m_coll_mesh_names.push_back("M113/Chassis_Hulls.obj");
break;
default:
break;
}
}
void
RankTwoTensorTest::rotateTest()
{
Real sqrt2 = 0.707106781187;
RealTensorValue rtv0(sqrt2, -sqrt2, 0, sqrt2, sqrt2, 0, 0, 0, 1); // rotation about "0" axis
RealTensorValue rtv1(sqrt2, 0, -sqrt2, 0, 1, 0, sqrt2, 0, sqrt2); // rotation about "1" axis
RealTensorValue rtv2(1, 0, 0, 0, sqrt2, -sqrt2, 0, sqrt2, sqrt2); // rotation about "2" axis
RankTwoTensor rot0(rtv0);
RankTwoTensor rot0T = rot0.transpose();
RankTwoTensor rot1(rtv1);
RankTwoTensor rot1T = rot1.transpose();
RankTwoTensor rot2(rtv2);
RankTwoTensor rot2T = rot2.transpose();
RankTwoTensor rot = rot0*rot1*rot2;
RankTwoTensor answer;
RankTwoTensor m3;
// the following "answer"s come from mathematica of course!
// rotate about "0" axis with RealTensorValue, then back again with RankTwoTensor
m3 = _m3;
answer = RankTwoTensor(-4, 3, 6.363961, 3, 0, -2.1213403, 6.363961, -2.1213403, 9);
m3.rotate(rtv0);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0, (m3 - answer).L2norm(), 0.0001);
m3.rotate(rot0T);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0, (m3 - _m3).L2norm(), 0.0001);
// rotate about "1" axis with RealTensorValue, then back again with RankTwoTensor
m3 = _m3;
answer = RankTwoTensor(2, 5.656854, -4, 5.656854, -5, -2.828427, -4, -2.828427, 8);
m3.rotate(rtv1);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0, (m3 - answer).L2norm(), 0.0001);
m3.rotate(rot1T);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0, (m3 - _m3).L2norm(), 0.0001);
// rotate about "2" axis with RealTensorValue, then back again with RankTwoTensor
m3 = _m3;
answer = RankTwoTensor(1, -sqrt2, 3.5355339, -sqrt2, 8, -7, 3.5355339, -7, -4);
m3.rotate(rtv2);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0, (m3 - answer).L2norm(), 0.0001);
m3.rotate(rot2T);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0, (m3 - _m3).L2norm(), 0.0001);
// rotate with "rot"
m3 = _m3;
answer = RankTwoTensor(-2.9675144, -6.51776695, 5.6213203, -6.51776695, 5.9319805, -2.0857864, 5.6213203, -2.0857864, 2.0355339);
m3.rotate(rot);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0, (m3 - answer).L2norm(), 0.0001);
}
osg::AnimationPath* createAnimationPath( float radius, float time )
{
osg::ref_ptr<osg::AnimationPath> path = new osg::AnimationPath;
path->setLoopMode( osg::AnimationPath::LOOP );
unsigned int numSamples = 32;
float delta_yaw = 2.0f * osg::PI/((float)numSamples - 1.0f);
float delta_time = time / (float)numSamples;
for ( unsigned int i=0; i<numSamples; ++i )
{
float yaw = delta_yaw * (float)i;
osg::Vec3 pos( sinf(yaw)*radius, cosf(yaw)*radius, 0.0f );
osg::Quat rot1( -yaw, osg::Z_AXIS );
osg::Quat rot2( -yaw, osg::X_AXIS );
path->insert( delta_time * (float)i, osg::AnimationPath::ControlPoint(pos, rot2*rot1) );
}
return path.release();
}
void energyEvalFunctionVisionFirst(int ndim, const NEWMAT::ColumnVector& x, double& fx, int& result)
{
//We have (ndim-3)/2 curve segments in x, where x(2*i-1) and x(2*i) are curvature and torsion of segment i
//the last 3 params represent the rotations to begin with
//rotate as in yaw, pitch, roll
Matrix3d rot1(Eigen::AngleAxisd(x(ndim-2), Vector3d::UnitZ()));
Vector3d axis2 = rot1*(Vector3d::UnitY());
Matrix3d rot2 = Eigen::AngleAxisd(x(ndim-1),axis2)*rot1;
Vector3d axis3 = rot2*(Vector3d::UnitX());
opt_params_vision.transform_back.corner(Eigen::TopLeft,3,3) = Eigen::AngleAxisd(x(ndim), axis3)*rot2;
opt_params_vision.curvature_before = x(1);
opt_params_vision.torsion_before = x(2);
energyEvalFunctionVision(ndim-3, x, fx, result);
}
__inline u32 byte_swap(const u32 x)
{
return rot1(x) & 0x00ff00ff | rot3(x) & 0xff00ff00;
}
glm::vec3 getEulerFromRotationMatrix(glm::mat3 rot, glm::vec3 curAngles) // order of rotation x->y->z Rx*Ry*Rz
// glm::translate(x....)
// glm::translate(y....)
// glm::translate(y....)
// x, y , z are angles
{
glm::vec3 radAngles = glm::radians(curAngles);
glm::mat3 rotTransposed = glm::transpose(rot);
float R11 = rotTransposed[0].x;
float R12 = rotTransposed[0].y;
float R13 = rotTransposed[0].z;
float R21 = rotTransposed[1].x;
float R22 = rotTransposed[1].y;
float R23 = rotTransposed[1].z;
float R31 = rotTransposed[2].x;
float R32 = rotTransposed[2].y;
float R33 = rotTransposed[2].z;
//printf("\nR11 = %f\n",R11);
//printf("R12 = %f\n",R12);
//printf("R13 = %f\n",R13);
//printf("R21 = %f\n",R21);
//printf("R22 = %f\n",R22);
//printf("R23 = %f\n",R23);
//printf("R31 = %f\n",R31);
//printf("R32 = %f\n",R32);
//printf("R33 = %f\n",R33);
// Find 2 possible y values
float y1 = asin(R13);
float y2 = M_PI - y1;
// Find possible z values
if (abs(y1-M_PI_2)>0.0001) // check whether cos(y) = 0 or not (y = pi/2)
{
// y != M_PI_2
float cy1 = cos(y1); float cy2 = cos(y2);
float z1 = atan2(-R12/cy1, R11/cy1);
float z2 = atan2(-R12/cy2, R11/cy2);
// Find 2 Possible x
float x1 = atan2(-R23/cy1, R33/cy1);
float x2 = atan2(-R23/cy2, R33/cy2);
glm::vec3 rot1(x1,y1,z1);
glm::vec3 rot2(x2,y2,z2);
glm::vec3 sub1 = rot1-radAngles;
glm::vec3 sub2 = rot2-radAngles;
//qDebug("Sub1 length = %f", glm::length(sub1));
//qDebug("Sub2 length = %f", glm::length(sub2));
int r1 = (int)(glm::degrees(sub1.x)) % 360;
int r2 = (int)(glm::degrees(sub2.x)) % 360;
if (r1<r2)
return glm::degrees(rot1);
else return glm::degrees(rot2);
}
else
{
// cos(y) = 0
// calculate possible x+z
float xz1 = atan2(R21,R22);
float x1 = radAngles.x; // remain x
float z1 = -x1 + xz1;
float xz2 = atan2(R12,R22);
float x2 = radAngles.x;
float z2 = x2 + xz2;
glm::vec3 rot1(x1,y1,z1);
glm::vec3 rot2(x2,y2,z2);
glm::vec3 sub1 = rot1-radAngles;
glm::vec3 sub2 = rot2-radAngles;
if (glm::length(sub1)<glm::length(sub2))
return glm::degrees(rot1);
else return glm::degrees(rot2);
}
}
/*******************************************************************************
* Function Name : AES_decrypt
* Description : Decrypts one block of 16 bytes
* Input : - input_pointer: input block address
* - expkey: decryption key
* Output : output_pointer: output block address
* Return : None
*******************************************************************************/
void AES_decrypt(u8* input_pointer, u32* output_pointer, u32* expkey)
{
register u32 s0;
register u32 s1;
register u32 s2;
register u32 s3;
register u32 t0;
register u32 t1;
register u32 t2;
register u32 t3;
register int r = 10 >> 1;
register u32* local_pointer = expkey + (40);
//added ***
register u32 copy0;
register u32 copy1;
register u32 copy2;
register u32 copy3;
copy0 = *input_pointer;
copy0 =copy0<<8;
copy0 |= *(input_pointer+1);
copy0 =copy0<<8;
copy0 |= *(input_pointer+2);
copy0 =copy0<<8;
copy0 |= *(input_pointer+3);
copy1 = *(input_pointer+4);
copy1 =copy1<<8;
copy1 |= *(input_pointer+5);
copy1 =copy1<<8;
copy1 |= *(input_pointer+6);
copy1 =copy1<<8;
copy1 |= *(input_pointer+7);
copy2 = *(input_pointer+8);
copy2 =copy2<<8;
copy2 |= *(input_pointer+9);
copy2 =copy2<<8;
copy2 |= *(input_pointer+10);
copy2 =copy2<<8;
copy2 |= *(input_pointer+11);
copy3 = *(input_pointer+12);
copy3 =copy3<<8;
copy3 |= *(input_pointer+13);
copy3 =copy3<<8;
copy3 |= *(input_pointer+14);
copy3 =copy3<<8;
copy3 |= *(input_pointer+15);
s0 = copy0^ local_pointer[0];
s1 = copy1 ^ local_pointer[1];
s2 = copy2 ^ local_pointer[2];
s3 = copy3 ^ local_pointer[3];
for (;;)
{
local_pointer -= 8;
t0 = dec_table[byte0(s0)] ^
rot1(dec_table[byte1(s3)]) ^
rot2(dec_table[byte2(s2)]) ^
rot3(dec_table[byte3(s1)]) ^
local_pointer[4];
t1 = dec_table[byte0(s1)] ^
rot1(dec_table[byte1(s0)]) ^
rot2(dec_table[byte2(s3)]) ^
rot3(dec_table[byte3(s2)]) ^
local_pointer[5];
t2 = dec_table[byte0(s2)] ^
rot1(dec_table[byte1(s1)]) ^
rot2(dec_table[byte2(s0)]) ^
rot3(dec_table[byte3(s3)]) ^
local_pointer[6];
t3 = dec_table[byte0(s3)] ^
rot1(dec_table[byte1(s2)]) ^
rot2(dec_table[byte2(s1)]) ^
rot3(dec_table[byte3(s0)]) ^
local_pointer[7];
if (--r == 0)
{
break;
}
s0 = dec_table[byte0(t0)] ^
rot1(dec_table[byte1(t3)]) ^
rot2(dec_table[byte2(t2)]) ^
rot3(dec_table[byte3(t1)]) ^
local_pointer[0];
s1 = dec_table[byte0(t1)] ^
rot1(dec_table[byte1(t0)]) ^
rot2(dec_table[byte2(t3)]) ^
rot3(dec_table[byte3(t2)]) ^
local_pointer[1];
s2 = dec_table[byte0(t2)] ^
rot1(dec_table[byte1(t1)]) ^
rot2(dec_table[byte2(t0)]) ^
rot3(dec_table[byte3(t3)]) ^
local_pointer[2];
s3 = dec_table[byte0(t3)] ^
rot1(dec_table[byte1(t2)]) ^
rot2(dec_table[byte2(t1)]) ^
rot3(dec_table[byte3(t0)]) ^
local_pointer[3];
}
output_pointer[0] = WORD8_TO_WORD32( InvSbox[byte0(t0)],
InvSbox[byte1(t3)],
InvSbox[byte2(t2)],
InvSbox[byte3(t1)]) ^ local_pointer[0];
output_pointer[1] = WORD8_TO_WORD32( InvSbox[byte0(t1)],
InvSbox[byte1(t0)],
InvSbox[byte2(t3)],
InvSbox[byte3(t2)]) ^ local_pointer[1];
output_pointer[2] = WORD8_TO_WORD32( InvSbox[byte0(t2)],
InvSbox[byte1(t1)],
InvSbox[byte2(t0)],
InvSbox[byte3(t3)]) ^ local_pointer[2];
output_pointer[3] = WORD8_TO_WORD32( InvSbox[byte0(t3)],
InvSbox[byte1(t2)],
InvSbox[byte2(t1)],
InvSbox[byte3(t0)]) ^ local_pointer[3];
}
/*******************************************************************************
* Function Name : AES_keyschedule_dec
* Description : According to key computes the expanded key (expkey) for AES128
* decryption.
* Input : key: user key
* Output : expkey: expanded key
* Return : None
*******************************************************************************/
void AES_keyschedule_dec(u8* key, u32* expkey)
{
register u32* local_pointer = expkey;
register int i = 0;
register u32 copy0;
register u32 copy1;
register u32 copy2;
register u32 copy3;
AES_keyschedule_enc(key,expkey);
local_pointer[0] = *key;
local_pointer[0] =local_pointer[0]<<8;
local_pointer[0] |= *(key+1);
local_pointer[0] =local_pointer[0]<<8;
local_pointer[0] |= *(key+2);
local_pointer[0] =local_pointer[0]<<8;
local_pointer[0] |= *(key+3);
local_pointer[1] = *(key+4);
local_pointer[1] =local_pointer[1]<<8;
local_pointer[1] |= *(key+5);
local_pointer[1] =local_pointer[1]<<8;
local_pointer[1] |= *(key+6);
local_pointer[1] =local_pointer[1]<<8;
local_pointer[1] |= *(key+7);
local_pointer[2] = *(key+8);
local_pointer[2] =local_pointer[2]<<8;
local_pointer[2] |= *(key+9);
local_pointer[2] =local_pointer[2]<<8;
local_pointer[2] |= *(key+10);
local_pointer[2] =local_pointer[2]<<8;
local_pointer[2] |= *(key+11);
local_pointer[3] = *(key+12);
local_pointer[3] =local_pointer[3]<<8;
local_pointer[3] |= *(key+13);
local_pointer[3] =local_pointer[3]<<8;
local_pointer[3] |= *(key+14);
local_pointer[3] =local_pointer[3]<<8;
local_pointer[3] |= *(key+15);
// local_pointer[0] = key[0];
// local_pointer[1] = key[1];
// local_pointer[2] = key[2];
// local_pointer[3] = key[3];
for (i = 1; i <10; i++)
{
local_pointer += 4;
copy0 = local_pointer[0];
copy1 = local_pointer[1];
copy2 = local_pointer[2];
copy3 = local_pointer[3];
local_pointer[0] = dec_table[Sbox[byte0(copy0)]] ^
rot1(dec_table[Sbox[byte1(copy0)]]) ^
rot2(dec_table[Sbox[byte2(copy0)]]) ^
rot3(dec_table[Sbox[byte3(copy0)]]);
local_pointer[1] = dec_table[Sbox[byte0(copy1)]] ^
rot1(dec_table[Sbox[byte1(copy1)]]) ^
rot2(dec_table[Sbox[byte2(copy1)]]) ^
rot3(dec_table[Sbox[byte3(copy1)]]);
local_pointer[2] = dec_table[Sbox[byte0(copy2)]] ^
rot1(dec_table[Sbox[byte1(copy2)]]) ^
rot2(dec_table[Sbox[byte2(copy2)]]) ^
rot3(dec_table[Sbox[byte3(copy2)]]);
local_pointer[3] = dec_table[Sbox[byte0(copy3)]] ^
rot1(dec_table[Sbox[byte1(copy3)]]) ^
rot2(dec_table[Sbox[byte2(copy3)]]) ^
rot3(dec_table[Sbox[byte3(copy3)]]);
}
}
/*******************************************************************************
* Function Name : AES_decrypt
* Description : Decrypts one block of 16 bytes
* Input : - input_pointer: input block address
* - expkey: decryption key
* Output : output_pointer: output block address
* Return : None
*******************************************************************************/
void AES_decrypt(ot_u32* input_pointer, ot_u32* output_pointer, ot_u32* expkey) {
#if (AES_USEHW == ENABLED)
#elif (AES_USEFAST == ENABLED)
register ot_u32 s0;
register ot_u32 s1;
register ot_u32 s2;
register ot_u32 s3;
register ot_u32 t0;
register ot_u32 t1;
register ot_u32 t2;
register ot_u32 t3;
register int r = 10 >> 1;
register ot_u32* local_pointer = expkey + (40);
s0 = input_pointer[0] ^ local_pointer[0];
s1 = input_pointer[1] ^ local_pointer[1];
s2 = input_pointer[2] ^ local_pointer[2];
s3 = input_pointer[3] ^ local_pointer[3];
for (;;)
{
local_pointer -= 8;
t0 = dec_table[byte0(s0)] ^
rot1(dec_table[byte1(s3)]) ^
rot2(dec_table[byte2(s2)]) ^
rot3(dec_table[byte3(s1)]) ^
local_pointer[4];
t1 = dec_table[byte0(s1)] ^
rot1(dec_table[byte1(s0)]) ^
rot2(dec_table[byte2(s3)]) ^
rot3(dec_table[byte3(s2)]) ^
local_pointer[5];
t2 = dec_table[byte0(s2)] ^
rot1(dec_table[byte1(s1)]) ^
rot2(dec_table[byte2(s0)]) ^
rot3(dec_table[byte3(s3)]) ^
local_pointer[6];
t3 = dec_table[byte0(s3)] ^
rot1(dec_table[byte1(s2)]) ^
rot2(dec_table[byte2(s1)]) ^
rot3(dec_table[byte3(s0)]) ^
local_pointer[7];
if (--r == 0)
{
break;
}
s0 = dec_table[byte0(t0)] ^
rot1(dec_table[byte1(t3)]) ^
rot2(dec_table[byte2(t2)]) ^
rot3(dec_table[byte3(t1)]) ^
local_pointer[0];
s1 = dec_table[byte0(t1)] ^
rot1(dec_table[byte1(t0)]) ^
rot2(dec_table[byte2(t3)]) ^
rot3(dec_table[byte3(t2)]) ^
local_pointer[1];
s2 = dec_table[byte0(t2)] ^
rot1(dec_table[byte1(t1)]) ^
rot2(dec_table[byte2(t0)]) ^
rot3(dec_table[byte3(t3)]) ^
local_pointer[2];
s3 = dec_table[byte0(t3)] ^
rot1(dec_table[byte1(t2)]) ^
rot2(dec_table[byte2(t1)]) ^
rot3(dec_table[byte3(t0)]) ^
local_pointer[3];
}
output_pointer[0] = WORD8_TO_WORD32( InvSbox[byte0(t0)],
InvSbox[byte1(t3)],
InvSbox[byte2(t2)],
InvSbox[byte3(t1)]) ^ local_pointer[0];
output_pointer[1] = WORD8_TO_WORD32( InvSbox[byte0(t1)],
InvSbox[byte1(t0)],
InvSbox[byte2(t3)],
InvSbox[byte3(t2)]) ^ local_pointer[1];
output_pointer[2] = WORD8_TO_WORD32( InvSbox[byte0(t2)],
InvSbox[byte1(t1)],
InvSbox[byte2(t0)],
InvSbox[byte3(t3)]) ^ local_pointer[2];
output_pointer[3] = WORD8_TO_WORD32( InvSbox[byte0(t3)],
InvSbox[byte1(t2)],
InvSbox[byte2(t1)],
InvSbox[byte3(t0)]) ^ local_pointer[3];
#elif (AES_USELITE == ENABLED)
/* Register: ask to the compiler to maintain this variable in the
processor's registers and don't store it in RAM */
register ot_u32 t0;
register ot_u32 t1;
register ot_u32 t2;
register ot_u32 t3;
register ot_u32 s0;
register ot_u32 s1;
register ot_u32 s2;
register ot_u32 s3;
register int r = 10; /* Count the round */
register ot_u32* local_pointer = expkey + 40;
ot_u32 f2,f4,f8;
s0 = input_pointer[0] ^ local_pointer[0];
s1 = input_pointer[1] ^ local_pointer[1];
s2 = input_pointer[2] ^ local_pointer[2];
s3 = input_pointer[3] ^ local_pointer[3];
/* First add key: before start the rounds */
local_pointer -= 8;
for (;;) /* Start round */
{
t0 = WORD8_TO_WORD32( InvSbox[byte0(s0)],
InvSbox[byte1(s3)],
InvSbox[byte2(s2)],
InvSbox[byte3(s1)]) ^ local_pointer[4];
t1 = WORD8_TO_WORD32( InvSbox[byte0(s1)],
InvSbox[byte1(s0)],
InvSbox[byte2(s3)],
InvSbox[byte3(s2)]) ^ local_pointer[5];
t2 = WORD8_TO_WORD32( InvSbox[byte0(s2)],
InvSbox[byte1(s1)],
InvSbox[byte2(s0)],
InvSbox[byte3(s3)]) ^ local_pointer[6];
t3 = WORD8_TO_WORD32( InvSbox[byte0(s3)],
InvSbox[byte1(s2)],
InvSbox[byte2(s1)],
InvSbox[byte3(s0)]) ^ local_pointer[7];
/*End of InvSbox, INVshiftRow, add key*/
s0=inv_mcol(t0);
s1=inv_mcol(t1);
s2=inv_mcol(t2);
s3=inv_mcol(t3);
/*End of INVMix column */
local_pointer -= 4; /*Follow the key sheduler to choose the right round key*/
if (--r == 1)
{
break;
}
}/*End of round*/
/*Start last round :is the only one different from the other*/
t0 = WORD8_TO_WORD32( InvSbox[byte0(s0)],
InvSbox[byte1(s3)],
InvSbox[byte2(s2)],
InvSbox[byte3(s1)]) ^ local_pointer[4];
t1 = WORD8_TO_WORD32( InvSbox[byte0(s1)],
InvSbox[byte1(s0)],
InvSbox[byte2(s3)],
InvSbox[byte3(s2)]) ^ local_pointer[5];
t2 = WORD8_TO_WORD32( InvSbox[byte0(s2)],
InvSbox[byte1(s1)],
InvSbox[byte2(s0)],
InvSbox[byte3(s3)]) ^ local_pointer[6];
t3 = WORD8_TO_WORD32( InvSbox[byte0(s3)],
InvSbox[byte1(s2)],
InvSbox[byte2(s1)],
InvSbox[byte3(s0)]) ^ local_pointer[7];
output_pointer[0] = t0;
output_pointer[1] = t1;
output_pointer[2] = t2;
output_pointer[3] = t3;
#endif
}
/*******************************************************************************
* Function Name : AES_encrypt
* Description : Encrypts one block of 16 bytes
* Input : - input_pointer: input block address
* - expkey: encryption key
* Output : output_pointer
* Return : None
*******************************************************************************/
void AES_encrypt(ot_u32* input_pointer, ot_u32* output_pointer, ot_u32* expkey) {
#if (AES_USEHW == ENABLED)
#elif (AES_USEFAST == ENABLED)
register ot_u32 s0;
register ot_u32 s1;
register ot_u32 s2;
register ot_u32 s3;
register ot_u32 t0;
register ot_u32 t1;
register ot_u32 t2;
register ot_u32 t3;
register int r = 10 >> 1;
register ot_u32* local_pointer = expkey;
s0 = input_pointer[0] ^ local_pointer[0];
s1 = input_pointer[1] ^ local_pointer[1];
s2 = input_pointer[2] ^ local_pointer[2];
s3 = input_pointer[3] ^ local_pointer[3];
for (;;) {
t0 = enc_table[byte0(s0)] ^
rot1(enc_table[byte1(s1)]) ^
rot2(enc_table[byte2(s2)]) ^
rot3(enc_table[byte3(s3)]) ^
local_pointer[4];
t1 = enc_table[byte0(s1)] ^
rot1(enc_table[byte1(s2)]) ^
rot2(enc_table[byte2(s3)]) ^
rot3(enc_table[byte3(s0)]) ^
local_pointer[5];
t2 = enc_table[byte0(s2)] ^
rot1(enc_table[byte1(s3)]) ^
rot2(enc_table[byte2(s0)]) ^
rot3(enc_table[byte3(s1)]) ^
local_pointer[6];
t3 = enc_table[byte0(s3)] ^
rot1(enc_table[byte1(s0)]) ^
rot2(enc_table[byte2(s1)]) ^
rot3(enc_table[byte3(s2)]) ^
local_pointer[7];
local_pointer += 8;
if (--r == 0) {
break;
}
s0 = enc_table[byte0(t0)] ^
rot1(enc_table[byte1(t1)]) ^
rot2(enc_table[byte2(t2)]) ^
rot3(enc_table[byte3(t3)]) ^
local_pointer[0];
s1 = enc_table[byte0(t1)] ^
rot1(enc_table[byte1(t2)]) ^
rot2(enc_table[byte2(t3)]) ^
rot3(enc_table[byte3(t0)]) ^
local_pointer[1];
s2 = enc_table[byte0(t2)] ^
rot1(enc_table[byte1(t3)]) ^
rot2(enc_table[byte2(t0)]) ^
rot3(enc_table[byte3(t1)]) ^
local_pointer[2];
s3 = enc_table[byte0(t3)] ^
rot1(enc_table[byte1(t0)]) ^
rot2(enc_table[byte2(t1)]) ^
rot3(enc_table[byte3(t2)]) ^
local_pointer[3];
}
output_pointer[0] = WORD8_TO_WORD32( Sbox[byte0(t0)],
Sbox[byte1(t1)],
Sbox[byte2(t2)],
Sbox[byte3(t3)]) ^ local_pointer[0];
output_pointer[1] = WORD8_TO_WORD32( Sbox[byte0(t1)],
Sbox[byte1(t2)],
Sbox[byte2(t3)],
Sbox[byte3(t0)]) ^ local_pointer[1];
output_pointer[2] = WORD8_TO_WORD32( Sbox[byte0(t2)],
Sbox[byte1(t3)],
Sbox[byte2(t0)],
Sbox[byte3(t1)]) ^ local_pointer[2];
output_pointer[3] = WORD8_TO_WORD32( Sbox[byte0(t3)],
Sbox[byte1(t0)],
Sbox[byte2(t1)],
Sbox[byte3(t2)]) ^ local_pointer[3];
#elif (AES_USELITE == ENABLED)
register ot_u32 t0;
register ot_u32 t1;
register ot_u32 t2;
register ot_u32 t3;
register ot_u32 s0;
register ot_u32 s1;
register ot_u32 s2;
register ot_u32 s3;
register int r = 10; // Round counter
register ot_u32* local_pointer = expkey;
s0 = input_pointer[0] ^ local_pointer[0];
s1 = input_pointer[1] ^ local_pointer[1];
s2 = input_pointer[2] ^ local_pointer[2];
s3 = input_pointer[3] ^ local_pointer[3];
local_pointer += 4;
// ADD KEY before start round
for (;;)
{
t0 = WORD8_TO_WORD32( Sbox[byte0(s0)],
Sbox[byte1(s1)],
Sbox[byte2(s2)],
Sbox[byte3(s3)]);
t1 = WORD8_TO_WORD32( Sbox[byte0(s1)],
Sbox[byte1(s2)],
Sbox[byte2(s3)],
Sbox[byte3(s0)]);
t2 = WORD8_TO_WORD32( Sbox[byte0(s2)],
Sbox[byte1(s3)],
Sbox[byte2(s0)],
Sbox[byte3(s1)]);
t3 = WORD8_TO_WORD32( Sbox[byte0(s3)],
Sbox[byte1(s0)],
Sbox[byte2(s1)],
Sbox[byte3(s2)]);
/*End of SBOX + Shift ROW*/
s0 = fwd_mcol(t0)^local_pointer[0];
s1 = fwd_mcol(t1)^local_pointer[1];
s2 = fwd_mcol(t2)^local_pointer[2];
s3 = fwd_mcol(t3)^local_pointer[3];
/*End of mix colomn*/
local_pointer += 4;
if ( --r == 1)
{
break;
}
}/*End for(;;)*/
/*Start Last round*/
t0 = WORD8_TO_WORD32( Sbox[byte0(s0)],
Sbox[byte1(s1)],
Sbox[byte2(s2)],
Sbox[byte3(s3)]);
t1 = WORD8_TO_WORD32( Sbox[byte0(s1)],
Sbox[byte1(s2)],
Sbox[byte2(s3)],
Sbox[byte3(s0)]);
t2 = WORD8_TO_WORD32( Sbox[byte0(s2)],
Sbox[byte1(s3)],
Sbox[byte2(s0)],
Sbox[byte3(s1)]);
t3 = WORD8_TO_WORD32( Sbox[byte0(s3)],
Sbox[byte1(s0)],
Sbox[byte2(s1)],
Sbox[byte3(s2)]);
t0 ^= local_pointer[0];
t1 ^= local_pointer[1];
t2 ^= local_pointer[2];
t3 ^= local_pointer[3];
/*Store of the result of encryption*/
output_pointer[0] = t0;
output_pointer[1] = t1;
output_pointer[2] = t2;
output_pointer[3] = t3;
#endif
}
__INLINE uint32_t byte_swap(uint32_t x){
return rot1(x) & 0x00ff00ff | rot3(x) & 0xff00ff00;
}
void test()
{
//tests
DRMatrix m1(DRMatrix::identity());
float mat[] = {1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f};
if(memcmp(m1, mat, sizeof(float)*16) != 0)
LOG_WARNING("matrix identity isn't valid");
DRMatrix m2 = m1.rotationX(30.0f);
DRMatrix m3 = DRMatrix::axis(DRVector3(1.0f, 0.0f, 0.0f),
DRVector3(0.0f, 1.0f, 0.0f),
DRVector3(0.0f, 0.0f, 1.0));
if(memcmp(m1, m3, sizeof(float)*16) != 0)
LOG_WARNING("matrix axis isn't valid");
DREngineLog.writeMatrixToLog(m1);
DREngineLog.writeMatrixToLog(m2);
DREngineLog.writeMatrixToLog(m3);
DRVector3 rot1(1.0f, 0.0f, 0.0f);
m2 = DRMatrix::rotationY(90.0f);
rot1 = rot1.transformCoords(m2);
DREngineLog.writeVector3ToLog(rot1, "1/0/0 90 Grad um y-Achse rotiert");
rot1 = rot1.transformCoords(m2.invert());
DREngineLog.writeVector3ToLog(rot1, "zurueckrotiert, 1/0/0 erwartet!");
DREngineLog.writeToLog("RekursionTest: %d", rekursionTest(0));
//Speicher test
/* LOG_INFO("Speichertest");
std::list<void*> pointer;
void* t = NULL;
u32 count = 0;
do
{
t = malloc(16384);
if(t) pointer.push_back(t);
count++;
if(count > 192073)
break;
} while(t);
DRLog.writeToLog("count: %d, %u kByte wurden reserviert!", count, count*16384/1024);
for(std::list<void*>::iterator it = pointer.begin(); it != pointer.end(); it++)
free(*it);
pointer.clear();
//* */
// Unit test
printf("\n");
Unit parsec(1.0, PARSEC);
Unit lj = parsec.convertTo(LIGHTYEAR);
DREngineLog.writeToLog("%s -> %s", parsec.print().data(), lj.print().data());
lj = Unit(1.0, LIGHTYEAR);
parsec = lj.convertTo(PARSEC);
DREngineLog.writeToLog("%s -> %s", lj.print().data(), parsec.print().data());
Unit ae = lj.convertTo(AE);
DREngineLog.writeToLog("%s -> %s", lj.print().data(), ae.print().data());
ae = parsec.convertTo(AE);
DREngineLog.writeToLog("%s -> %s", parsec.print().data(), ae.print().data());
parsec = ae.convertTo(PARSEC);
DREngineLog.writeToLog("%s -> %s", ae.print().data(), parsec.print().data());
Unit m = parsec.convertTo(M);
DREngineLog.writeToLog("%s -> %s", parsec.print().data(), m.print().data());
Unit kpc(1.0, KILOPARSEC);
m = kpc.convertTo(M);
DREngineLog.writeToLog("%s -> %s", kpc.print().data(), m.print().data());
m = Unit(1.0, M);
kpc = m.convertTo(KILOPARSEC);
DREngineLog.writeToLog("%s -> %s", m.print().data(), kpc.print().data());
printf("\n");
Unit aes(0.005, AE);
DREngineLog.writeToLog("%s -> %s", aes.print().data(), aes.convertTo(KM).print().data());
//Vector Unit Test
Vector3Unit u1(100, 200, 70, M), u2(1, 0, 0, KILOPARSEC), u3(100, 20, 17, LIGHTYEAR);
u1.print("u1");
u2.print("u2");
u3.print("u3");
u1 *= Unit(20, KM);
u1.print("u1* 20 km");
Vector3Unit(u1 + u2).print("u1+u2");
Vector3Unit(u2+u3).print("u2+u3");
Vector3Unit(u1*Unit(1, LIGHTYEAR)).print("u1*1 Lichtjahr");
DRVector3 v(1.0f, 7.0f, 2.0f);
DREngineLog.writeVector3ToLog(v, "init");
v = v.normalize();
DREngineLog.writeVector3ToLog(v, "normalized");
v *= 7.0f;
DREngineLog.writeVector3ToLog(v, "multiplikator");
// ---------------------------------- ReferenzHolder Test --------------------------------
DREngineLog.writeToLog("DRIndexReferenzHolder test");
DRIndexReferenzHolder referenzHolder(10);
uint tests[10];
tests[0] = referenzHolder.getFree();
referenzHolder.add(tests[0]);
tests[1] = referenzHolder.getFree();
DREngineLog.writeToLog("index1 (0): %d, index2 (1): %d", tests[0], tests[1]);
referenzHolder.remove(tests[0]);
tests[2] = referenzHolder.getFree();
referenzHolder.remove(tests[1]);
tests[3] = referenzHolder.getFree();
DREngineLog.writeToLog("index3 (2): %d, index4 (1): %d", tests[2], tests[3]);
for(int i = 0; i < 5; i++)
tests[4+i] = referenzHolder.getFree();
referenzHolder.remove(tests[7]);
tests[9] = referenzHolder.getFree();
DREngineLog.writeToLog("index10: (6): %d", tests[9]);
DRTextureManager::Instance().test();
// Random Test
}
