pcover reduce(pset_family F, pset_family D)
{
register pcube last, p, cunder, *FD;
/* Order the cubes */
if (use_random_order)
F = random_order(F);
else {
F = toggle ? sort_reduce(F) : mini_sort(F, (qsort_compare_func) descend);
toggle = ! toggle;
}
/* Try to reduce each cube */
FD = cube2list(F, D);
foreach_set(F, last, p) {
cunder = reduce_cube(FD, p); /* reduce the cube */
if (setp_equal(cunder, p)) { /* see if it actually did */
SET(p, ACTIVE); /* cube remains active */
SET(p, PRIME); /* cube remains prime ? */
} else {
if (debug & REDUCE) {
printf("REDUCE: %s to %s %s\n",
pc1(p), pc2(cunder), print_time(ptime()));
}
set_copy(p, cunder); /* save reduced version */
RESET(p, PRIME); /* cube is no longer prime */
if (setp_empty(cunder))
RESET(p, ACTIVE); /* if null, kill the cube */
else
SET(p, ACTIVE); /* cube is active */
}
free_cube(cunder);
}
void BulletFrom1DLocalFrameR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
y.setValue(0, _contactPoints->getDistance());
btVector3 posa = _contactPoints->getPositionWorldOnA();
btVector3 posb = _contactPoints->getPositionWorldOnB();
(*pc1())(0) = posa[0];
(*pc1())(1) = posa[1];
(*pc1())(2) = posa[2];
(*pc2())(0) = posb[0];
(*pc2())(1) = posb[1];
(*pc2())(2) = posb[2];
(*nc())(0) = _contactPoints->m_normalWorldOnB[0];
(*nc())(1) = _contactPoints->m_normalWorldOnB[1];
(*nc())(2) = _contactPoints->m_normalWorldOnB[2];;
}
int main()
{
PriorityCustomer2 pc1;
PriorityCustomer2 pc2(pc1);
PriorityCustomer2 pc3;
pc3 = pc1;
return 0;
}
void BulletR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
DEBUG_BEGIN("BulletR::computeh(...)\n");
NewtonEulerR::computeh(time, q0, y);
DEBUG_PRINT("start of computeh\n");
btVector3 posa = _contactPoints->getPositionWorldOnA();
btVector3 posb = _contactPoints->getPositionWorldOnB();
if (_flip) {
posa = _contactPoints->getPositionWorldOnB();
posb = _contactPoints->getPositionWorldOnA();
}
(*pc1())(0) = posa[0];
(*pc1())(1) = posa[1];
(*pc1())(2) = posa[2];
(*pc2())(0) = posb[0];
(*pc2())(1) = posb[1];
(*pc2())(2) = posb[2];
{
y.setValue(0, _contactPoints->getDistance());
(*nc())(0) = _contactPoints->m_normalWorldOnB[0] * (_flip ? -1 : 1);
(*nc())(1) = _contactPoints->m_normalWorldOnB[1] * (_flip ? -1 : 1);
(*nc())(2) = _contactPoints->m_normalWorldOnB[2] * (_flip ? -1 : 1);
}
DEBUG_PRINTF("distance : %g\n", y.getValue(0));
DEBUG_PRINTF("position on A : %g,%g,%g\n", posa[0], posa[1], posa[2]);
DEBUG_PRINTF("position on B : %g,%g,%g\n", posb[0], posb[1], posb[2]);
DEBUG_PRINTF("normal on B : %g,%g,%g\n", (*nc())(0), (*nc())(1), (*nc())(2));
DEBUG_END("BulletR::computeh(...)\n");
}
Fixture()
: application_context("foo")
{
odil::pdu::PresentationContextAC pc1(3, "transfer_syntax", 1);
odil::pdu::PresentationContextAC pc2(5, "transfer_syntax_2", 2);
this->presentation_contexts = {pc1, pc2};
this->user_information.set_sub_items<odil::pdu::MaximumLength>(
{ { 0x12345678 } });
this->user_information.set_sub_items<odil::pdu::UserIdentityAC>(
{ { "bar" } });
}
BulletR::BulletR(SP::btManifoldPoint point, bool flip) :
NewtonEulerFrom3DLocalFrameR(),
_contactPoints(point),
_flip(flip)
{
btVector3 posa = _contactPoints->getPositionWorldOnA();
btVector3 posb = _contactPoints->getPositionWorldOnB();
if (flip) {
posa = _contactPoints->getPositionWorldOnB();
posb = _contactPoints->getPositionWorldOnA();
}
(*pc1())(0) = posa[0];
(*pc1())(1) = posa[1];
(*pc1())(2) = posa[2];
(*pc2())(0) = posb[0];
(*pc2())(1) = posb[1];
(*pc2())(2) = posb[2];
(*nc())(0) = _contactPoints->m_normalWorldOnB[0] * (flip ? -1 : 1);
(*nc())(1) = _contactPoints->m_normalWorldOnB[1] * (flip ? -1 : 1);
(*nc())(2) = _contactPoints->m_normalWorldOnB[2] * (flip ? -1 : 1);
}
int main() try {
edm::ParameterSet dummyPset;
dummyPset.registerIt();
edm::ParameterSetID id = dummyPset.id();
{
edm::ProcessConfiguration pc1;
pc1.setParameterSetID(id);
assert(pc1 == pc1);
}
{
edm::ProcessConfiguration pc1;
edm::ProcessConfiguration pc2;
pc1.setParameterSetID(id);
pc2.setParameterSetID(id);
assert(pc1 == pc2);
}
{
edm::ProcessConfiguration pc1;
edm::ProcessConfiguration pc2("reco2", edm::ParameterSetID(), std::string(), std::string());
edm::ProcessConfiguration pc3("reco3", edm::ParameterSetID(), std::string(), std::string());
edm::ProcessConfiguration pc4("reco2", edm::ParameterSetID(), std::string(), std::string());
pc1.setParameterSetID(id);
pc2.setParameterSetID(id);
pc3.setParameterSetID(id);
pc4.setParameterSetID(id);
edm::ProcessConfigurationID id1 = pc1.id();
edm::ProcessConfigurationID id2 = pc2.id();
edm::ProcessConfigurationID id3 = pc3.id();
assert(id1 != id2);
assert(id2 != id3);
assert(id3 != id1);
edm::ProcessConfigurationID id4 = pc4.id();
assert(pc4 == pc2);
assert (id4 == id2);
}
return 0;
}
catch(cms::Exception const& e) {
std::cerr << e.explainSelf() << std::endl;
return 1;
}
catch(std::exception const& e) {
std::cerr << e.what() << std::endl;
return 1;
}
static pset_family reduce_gasp(pset_family F, pset_family D)
{
pset p, last, cunder, *FD;
pset_family G;
G = sf_new(F->count, cube.size);
FD = cube2list(F, D);
for( p=F->data, last= p+F->count*F->wsize; p< last; p+=F->wsize) {
cunder = reduce_cube(FD, p);
if (setp_empty(cunder)) {
fatal("empty reduction in reduce_gasp, shouldn't happen");
} else if (setp_equal(cunder, p)) {
(cunder[0] |= ( 0x8000)); G = sf_addset(G, p); } else {
(cunder[0] &= ~ ( 0x8000)); G = sf_addset(G, cunder);
}
if (debug & 0x0010) {
printf("REDUCE_GASP: %s reduced to %s\n", pc1(p), pc2(cunder));
}
((cunder) ? (free((char *) (cunder)), (cunder) = 0) : 0);
}
((FD[0]) ? (free((char *) (FD[0])), (FD[0]) = 0) : 0); ((FD) ? (free((char *) (FD)), (FD) = 0) : 0);;
return G;
}
TopAbs_State GEOMAlgo_FinderShapeOnQuad::GetPointState(const gp_Pnt& aP)
{
// Return IN if aP has TopAbs_IN with all sides.
// In the case of concave quadrangle, return IN if
// aP is OUT of only one concave side
double nbIn = 0.;
for ( int i = 0; i < myPlanes.size(); ++i )
{
TopAbs_State aSt;
GEOMAlgo_SurfaceTools::GetState(aP, myPlanes[i], myTolerance, aSt);
if ( aSt == TopAbs_IN )
{
nbIn += myConcaveSide[i] ? 0.5 : 1.0;
}
else if ( aSt == TopAbs_ON )
{
// check that aP is between quadrangle corners
Handle(Geom_Plane) aSidePlane = Handle(Geom_Plane)::DownCast( myPlanes[i].Surface() );
gp_Vec aSideNorm = aSidePlane->Axis().Direction();
gp_Vec aSideVec = myQuadNormal ^ aSideNorm;
gp_Vec c1p ( myPoints[i], aP );
gp_Vec pc2 ( aP, myPoints[i+1] );
if ( aSideVec * c1p >= 0. && aSideVec * pc2 >= 0. )
return TopAbs_ON;
// consider to be IN (???????????)
//nbIn += myConcaveSide[i] ? 0.5 : 1.0;
}
}
Standard_Real inThreshold = myPlanes.size(); // usually 4.0
if ( myConcaveQuad )
inThreshold = 2.5; // 1.0 + 1.0 + 0.5
if ( nbIn >= inThreshold )
return TopAbs_IN;
return TopAbs_OUT;
}
static pcover compl_merge(pset *T1, pset_family L, pset_family R, register pset cl, register pset cr, int var, int lifting)
/* Original ON-set */
/* Complement from each recursion branch */
/* cubes used for cofactoring */
/* splitting variable */
/* whether to perform lifting or not */
{
register pcube p, last, pt;
pcover T, Tbar;
pcube *L1, *R1;
if (debug & COMPL) {
printf("compl_merge: left %d, right %d\n", L->count, R->count);
printf("%s (cl)\n%s (cr)\nLeft is\n", pc1(cl), pc2(cr));
cprint(L);
printf("Right is\n");
cprint(R);
}
/* Intersect each cube with the cofactored cube */
foreach_set(L, last, p) {
INLINEset_and(p, p, cl);
SET(p, ACTIVE);
}
static set_family_t *compl_merge(set **T1, set_family_t *L, set_family_t *R, set *cl, set *cr, int var, int lifting)
/* Original ON-set */
/* Complement from each recursion branch */
/* cubes used for cofactoring */
/* splitting variable */
/* whether to perform lifting or not */
{
set *p, *last, *pt;
set_family_t *T, *Tbar;
set **L1, **R1;
if (debug & COMPL) {
printf("compl_merge: left %d, right %d\n", L->count, R->count);
printf("%s (cl)\n%s (cr)\nLeft is\n", pc1(cl), pc2(cr));
cprint(L);
printf("Right is\n");
cprint(R);
}
// Intersect each cube with the cofactored cube
foreach_set(L, last, p) {
set_and(p, p, cl);
SET(p, ACTIVE);
}
void WireframeTriangleRasterizer::drawLine(const math::vertex &p1, const math::vertex &p2, FrameBuffer *fb)
{
math::vec3 pc1(p1.p), pc2(p2.p);
int cols = fb->width();
if (clipLine(pc1, pc2, fb))
{
int error;
int x0 = pc1.x, x1 = pc2.x;
int y0 = pc1.y, y1 = pc2.y;
int pixNum;
pixNum = y0 * cols + x0;
int dx = x1 - x0;
int dy = y1 - y0;
int xInc, yInc;
if (dx >= 0)
xInc = 1;
else
{
xInc = -1;
dx = -dx;
}
if (dy >= 0)
yInc = cols;
else
{
yInc = -cols;
dy = -dy;
}
int dx2 = dx * 2;
int dy2 = dy * 2;
if (dx > dy)
{
error = dy2 - dx;
for (int index = 0; index <= dx; index++)
{
fb->wpixel(pixNum, p1.color);
if (error >= 0)
{
error -= dx2;
pixNum += yInc;
}
error += dy2;
pixNum += xInc;
}
}
else
{
error = dx2 - dy;
for (int index = 0; index <= dy; index++)
{
fb->wpixel(pixNum, p1.color);
if (error >= 0)
{
error -= dy2;
pixNum += xInc;
}
error += dx2;
pixNum += yInc;
}
}
}
}
TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
{
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();
ks.update();
robot_state::RobotState ks_const(kmodel);
ks_const.setToDefaultValues();
ks_const.update();
robot_state::Transforms &tf = ps->getTransformsNonConst();
kinematic_constraints::JointConstraint jc1(kmodel);
std::map<std::string, double> state_values;
moveit_msgs::JointConstraint torso_constraint;
torso_constraint.joint_name = "torso_lift_joint";
torso_constraint.position = ks.getVariablePosition("torso_lift_joint");
torso_constraint.tolerance_above = 0.01;
torso_constraint.tolerance_below = 0.01;
torso_constraint.weight = 1.0;
EXPECT_TRUE(jc1.configure(torso_constraint));
kinematic_constraints::JointConstraint jc2(kmodel);
moveit_msgs::JointConstraint jcm2;
jcm2.joint_name = "r_elbow_flex_joint";
jcm2.position = ks.getVariablePosition("r_elbow_flex_joint");
jcm2.tolerance_above = 0.01;
jcm2.tolerance_below = 0.01;
jcm2.weight = 1.0;
EXPECT_TRUE(jc2.configure(jcm2));
moveit_msgs::PositionConstraint pcm;
pcm.link_name = "l_wrist_roll_link";
pcm.target_point_offset.x = 0;
pcm.target_point_offset.y = 0;
pcm.target_point_offset.z = 0;
pcm.constraint_region.primitives.resize(1);
pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
pcm.constraint_region.primitives[0].dimensions.resize(1);
pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
pcm.constraint_region.primitive_poses.resize(1);
pcm.constraint_region.primitive_poses[0].position.x = 0.55;
pcm.constraint_region.primitive_poses[0].position.y = 0.2;
pcm.constraint_region.primitive_poses[0].position.z = 1.25;
pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
pcm.weight = 1.0;
pcm.header.frame_id = kmodel->getModelFrame();
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "l_wrist_roll_link";
ocm.header.frame_id = kmodel->getModelFrame();
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.4;
ocm.weight = 1.0;
std::vector<kinematic_constraints::JointConstraint> js;
js.push_back(jc1);
boost::shared_ptr<constraint_samplers::JointConstraintSampler> jcsp(new constraint_samplers::JointConstraintSampler(ps, "arms_and_torso"));
EXPECT_TRUE(jcsp->configure(js));
std::vector<kinematic_constraints::JointConstraint> js2;
js2.push_back(jc2);
boost::shared_ptr<constraint_samplers::JointConstraintSampler> jcsp2(new constraint_samplers::JointConstraintSampler(ps, "arms"));
EXPECT_TRUE(jcsp2->configure(js2));
kinematic_constraints::PositionConstraint pc(kmodel);
EXPECT_TRUE(pc.configure(pcm, tf));
kinematic_constraints::OrientationConstraint oc(kmodel);
EXPECT_TRUE(oc.configure(ocm, tf));
boost::shared_ptr<constraint_samplers::IKConstraintSampler> iksp(new constraint_samplers::IKConstraintSampler(ps, "left_arm"));
EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
EXPECT_TRUE(iksp->isValid());
std::vector<constraint_samplers::ConstraintSamplerPtr> cspv;
cspv.push_back(jcsp2);
cspv.push_back(iksp);
cspv.push_back(jcsp);
constraint_samplers::UnionConstraintSampler ucs(ps, "arms_and_torso", cspv);
//should have reordered to place whole body first
constraint_samplers::JointConstraintSampler* jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[0].get());
EXPECT_TRUE(jcs);
EXPECT_EQ(jcs->getJointModelGroup()->getName(), "arms_and_torso");
constraint_samplers::JointConstraintSampler* jcs2 = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[1].get());
EXPECT_TRUE(jcs2);
EXPECT_EQ(jcs2->getJointModelGroup()->getName(), "arms");
for (int t = 0 ; t < 100; ++t)
{
EXPECT_TRUE(ucs.sample(ks, ks_const, 100));
ks.update();
ks_const.update();
EXPECT_TRUE(jc1.decide(ks).satisfied);
EXPECT_TRUE(jc2.decide(ks).satisfied);
EXPECT_TRUE(pc.decide(ks).satisfied);
}
//now we add a position constraint on right arm
pcm.link_name = "r_wrist_roll_link";
ocm.link_name = "r_wrist_roll_link";
cspv.clear();
kinematic_constraints::PositionConstraint pc2(kmodel);
EXPECT_TRUE(pc2.configure(pcm, tf));
kinematic_constraints::OrientationConstraint oc2(kmodel);
EXPECT_TRUE(oc2.configure(ocm, tf));
boost::shared_ptr<constraint_samplers::IKConstraintSampler> iksp2(new constraint_samplers::IKConstraintSampler(ps, "right_arm"));
EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2)));
EXPECT_TRUE(iksp2->isValid());
//totally disjoint, so should break ties based on alphabetical order
cspv.clear();
cspv.push_back(iksp2);
cspv.push_back(iksp);
constraint_samplers::UnionConstraintSampler ucs2(ps, "arms_and_torso", cspv);
constraint_samplers::IKConstraintSampler* ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs2.getSamplers()[0].get());
ASSERT_TRUE(ikcs_test);
EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "left_arm");
//now we make left depends on right, right should stay first
pcm.link_name = "l_wrist_roll_link";
ocm.link_name = "l_wrist_roll_link";
pcm.header.frame_id = "r_wrist_roll_link";
ocm.header.frame_id = "r_wrist_roll_link";
EXPECT_TRUE(pc.configure(pcm, tf));
EXPECT_TRUE(oc.configure(ocm, tf));
ASSERT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
cspv.clear();
cspv.push_back(iksp2);
cspv.push_back(iksp);
constraint_samplers::UnionConstraintSampler ucs3(ps, "arms_and_torso", cspv);
ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs3.getSamplers()[0].get());
EXPECT_TRUE(ikcs_test);
EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "right_arm");
}
/*-----------------------------------------------------------
DES Gernerate Sub Key procedure
Description:
-----------------------------------------------------------*/
void Gsubkey(char key[8],char subkey[16][6])
{
int i;
char cup[4], dup[4];
char ci[4], di[4];
char lsi[16];
union hbyte ip1;
union hbyte ipr;
ip1.abyte=0;
ipr.abyte=0;
/* Initial LSi */
lsi[0]=lsi[1]=lsi[8]=lsi[15]=1;
lsi[2]=lsi[3]=lsi[4]=lsi[5]=lsi[6]=lsi[7]=2;
lsi[9]=lsi[10]=lsi[11]=lsi[12]=lsi[13]=lsi[14]=2;
/* Getout 1 bit of 1 byte: all 56 bytes */
/* Through PC-1, Get C0, 28 bits */
ip1.abyte=key[0]; ipr.ibyte.bit7=ip1.ibyte.bit7; /* 57 bit */
ip1.abyte=key[1]; ipr.ibyte.bit6=ip1.ibyte.bit7; /* 49 bit */
ip1.abyte=key[2]; ipr.ibyte.bit5=ip1.ibyte.bit7; /* 41 bit */
ip1.abyte=key[3]; ipr.ibyte.bit4=ip1.ibyte.bit7; /* 33 bit */
ip1.abyte=key[4]; ipr.ibyte.bit3=ip1.ibyte.bit7; /* 25 bit */
ip1.abyte=key[5]; ipr.ibyte.bit2=ip1.ibyte.bit7; /* 17 bit */
ip1.abyte=key[6]; ipr.ibyte.bit1=ip1.ibyte.bit7; /* 9 bit */
ip1.abyte=key[7]; ipr.ibyte.bit0=ip1.ibyte.bit7; /* 1 bit */
ci[3]=ipr.abyte;
ip1.abyte=key[0]; ipr.ibyte.bit7=ip1.ibyte.bit6; /* 58 bit */
ip1.abyte=key[1]; ipr.ibyte.bit6=ip1.ibyte.bit6; /* 50 bit */
ip1.abyte=key[2]; ipr.ibyte.bit5=ip1.ibyte.bit6; /* 42 bit */
ip1.abyte=key[3]; ipr.ibyte.bit4=ip1.ibyte.bit6; /* 34 bit */
ip1.abyte=key[4]; ipr.ibyte.bit3=ip1.ibyte.bit6; /* 26 bit */
ip1.abyte=key[5]; ipr.ibyte.bit2=ip1.ibyte.bit6; /* 18 bit */
ip1.abyte=key[6]; ipr.ibyte.bit1=ip1.ibyte.bit6; /* 10 bit */
ip1.abyte=key[7]; ipr.ibyte.bit0=ip1.ibyte.bit6; /* 2 bit */
ci[2]=ipr.abyte;
ip1.abyte=key[0]; ipr.ibyte.bit7=ip1.ibyte.bit5; /* 59 bit */
ip1.abyte=key[1]; ipr.ibyte.bit6=ip1.ibyte.bit5; /* 51 bit */
ip1.abyte=key[2]; ipr.ibyte.bit5=ip1.ibyte.bit5; /* 43 bit */
ip1.abyte=key[3]; ipr.ibyte.bit4=ip1.ibyte.bit5; /* 35 bit */
ip1.abyte=key[4]; ipr.ibyte.bit3=ip1.ibyte.bit5; /* 27 bit */
ip1.abyte=key[5]; ipr.ibyte.bit2=ip1.ibyte.bit5; /* 19 bit */
ip1.abyte=key[6]; ipr.ibyte.bit1=ip1.ibyte.bit5; /* 11 bit */
ip1.abyte=key[7]; ipr.ibyte.bit0=ip1.ibyte.bit5; /* 3 bit */
ci[1]=ipr.abyte;
ip1.abyte=key[0]; ipr.ibyte.bit7=ip1.ibyte.bit4; /* 60 bit */
ip1.abyte=key[1]; ipr.ibyte.bit6=ip1.ibyte.bit4; /* 52 bit */
ip1.abyte=key[2]; ipr.ibyte.bit5=ip1.ibyte.bit4; /* 44 bit */
ip1.abyte=key[3]; ipr.ibyte.bit4=ip1.ibyte.bit4; /* 36 bit */
ci[0]=ipr.abyte;
/* Through PC-1, Get D0, 28 bits */
ip1.abyte=key[0]; ipr.ibyte.bit7=ip1.ibyte.bit1; /* 63 bit */
ip1.abyte=key[1]; ipr.ibyte.bit6=ip1.ibyte.bit1; /* 55 bit */
ip1.abyte=key[2]; ipr.ibyte.bit5=ip1.ibyte.bit1; /* 47 bit */
ip1.abyte=key[3]; ipr.ibyte.bit4=ip1.ibyte.bit1; /* 39 bit */
ip1.abyte=key[4]; ipr.ibyte.bit3=ip1.ibyte.bit1; /* 31 bit */
ip1.abyte=key[5]; ipr.ibyte.bit2=ip1.ibyte.bit1; /* 23 bit */
ip1.abyte=key[6]; ipr.ibyte.bit1=ip1.ibyte.bit1; /* 15 bit */
ip1.abyte=key[7]; ipr.ibyte.bit0=ip1.ibyte.bit1; /* 7 bit */
di[3]=ipr.abyte;
ip1.abyte=key[0]; ipr.ibyte.bit7=ip1.ibyte.bit2; /* 62 bit */
ip1.abyte=key[1]; ipr.ibyte.bit6=ip1.ibyte.bit2; /* 54 bit */
ip1.abyte=key[2]; ipr.ibyte.bit5=ip1.ibyte.bit2; /* 46 bit */
ip1.abyte=key[3]; ipr.ibyte.bit4=ip1.ibyte.bit2; /* 38 bit */
ip1.abyte=key[4]; ipr.ibyte.bit3=ip1.ibyte.bit2; /* 30 bit */
ip1.abyte=key[5]; ipr.ibyte.bit2=ip1.ibyte.bit2; /* 22 bit */
ip1.abyte=key[6]; ipr.ibyte.bit1=ip1.ibyte.bit2; /* 14 bit */
ip1.abyte=key[7]; ipr.ibyte.bit0=ip1.ibyte.bit2; /* 6 bit */
di[2]=ipr.abyte;
ip1.abyte=key[0]; ipr.ibyte.bit7=ip1.ibyte.bit3; /* 61 bit */
ip1.abyte=key[1]; ipr.ibyte.bit6=ip1.ibyte.bit3; /* 53 bit */
ip1.abyte=key[2]; ipr.ibyte.bit5=ip1.ibyte.bit3; /* 45 bit */
ip1.abyte=key[3]; ipr.ibyte.bit4=ip1.ibyte.bit3; /* 37 bit */
ip1.abyte=key[4]; ipr.ibyte.bit3=ip1.ibyte.bit3; /* 29 bit */
ip1.abyte=key[5]; ipr.ibyte.bit2=ip1.ibyte.bit3; /* 21 bit */
ip1.abyte=key[6]; ipr.ibyte.bit1=ip1.ibyte.bit3; /* 13 bit */
ip1.abyte=key[7]; ipr.ibyte.bit0=ip1.ibyte.bit3; /* 5 bit */
di[1]=ipr.abyte;
ip1.abyte=key[4]; ipr.ibyte.bit7=ip1.ibyte.bit4; /* 28 bit */
ip1.abyte=key[5]; ipr.ibyte.bit6=ip1.ibyte.bit4; /* 20 bit */
ip1.abyte=key[6]; ipr.ibyte.bit5=ip1.ibyte.bit4; /* 12 bit */
ip1.abyte=key[7]; ipr.ibyte.bit4=ip1.ibyte.bit4; /* 4 bit */
di[0]=ipr.abyte;
for(i=0;i<16;i=i+1) {
cup[3]=ci[3]; cup[2]=ci[2]; cup[1]=ci[1]; cup[0]=ci[0];
dup[3]=di[3]; dup[2]=di[2]; dup[1]=di[1]; dup[0]=di[0];
/* Generate 16 Subkey */
rotatebits(cup,ci,lsi[i]);
rotatebits(dup,di,lsi[i]);
pc2(ci,di,subkey[i]);
}
return;
}