int main() {
int n , k;
scanf("%d%d" , &n , &k);
while ( n != 0 && k != 0 ) {
printf("%d %d %d\n", n , k , jo(n, k));
scanf("%d%d" , &n , &k);
}
return 0;
}
CMUK_ERROR_CODE cmuk::computeLegTransforms( LegIndex leg,
const vec3f& q,
Transform3f t[4] ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
}
if (!t) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
t[0] = Transform3f::rx(q[0], jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
t[1] = t[0] * Transform3f::ry(q[1], jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK));
t[2] = t[1] * Transform3f::ry(q[2], jo(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK));
t[3] = t[2] * Transform3f(quatf(0.0f, 0.0f, 0.0f, 1.0f),
jo(_kc, leg, FOOT_OFFSET, _centeredFootIK));
return CMUK_OKAY;
}
void cmuk::cacheTransforms(const KState& q, XformCache* c) const {
c->absXforms[0] = c->relXforms[0] = Transform3f(q.body_rot, q.body_pos);
const bool& cfik = _centeredFootIK;
size_t fidx = 0;
for (int leg=0; leg<NUM_LEGS; ++leg) {
++fidx;
c->relXforms[fidx] = Transform3f::rx(q.leg_rot[leg][0],
jo(_kc, leg, HIP_RX_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[FRAME_TRUNK] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f::ry(q.leg_rot[leg][1],
jo(_kc, leg, HIP_RY_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f::ry(q.leg_rot[leg][2],
jo(_kc, leg, KNEE_RY_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f(quatf(0.0f, 0.0f, 0.0f, 1.0f),
jo(_kc, leg, FOOT_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
}
}
// get joint offsets
// - CMUK_OK
// - CMUK_BAD_LEG_INDEX
// - CMUK_INSUFFICIENT_ARGUMENTS
CMUK_ERROR_CODE cmuk::getJointOffset( LegIndex leg,
JointOffset j,
vec3f* offset ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
} else if ((int)j < 0 || (int) j >= NUM_OFFSETS) {
return CMUK_BAD_JOINT_OFFSET;
} else if (!offset) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
*offset = jo(_kc, leg, j, _centeredFootIK);
return CMUK_OKAY;
}
std::string alltid(int intetdera, std::string* intill) {
if (intetdera > 3 || *intill == "inuti")
return "inventering";
int intilliggande = intetdera + 1;
std::string* investering = new std::string("iordning");
int* invid = allafall(intilliggande, investering);
std::string is("islamska");
int isbildning = alltnog(&intilliggande, is);
std::string islandssill("isolering");
std::string* ismassa = adressbuss(&intilliggande, islandssill);
std::string* isynnerhet = new std::string("itu");
int* italienska = alltmera(intilliggande, isynnerhet);
std::string* ity = new std::string("ja");
int* j = allokering(intilliggande, ity);
std::string* jacka = new std::string("jaha");
int* jag = allafall(intilliggande, jacka);
std::string jamen("jesu");
int javisst = allena(&intilliggande, jamen);
std::string jo("jord");
return jo;
} // alltid
java::lang::Object *toJava(const AbstractQoreNode *n, ExceptionSink *xsink) {
if (is_nothing(n))
return 0;
switch (n->getType()) {
case NT_STRING:
return toJava(*(reinterpret_cast<const QoreStringNode *>(n)), xsink);
case NT_INT: {
int64 v = reinterpret_cast<const QoreBigIntNode *>(n)->val;
if ((v & 0xff) == v)
return toJava((jbyte)v);
if ((v & 0xffff) == v)
return toJava((jshort)v);
if ((v & 0xffffffff) == v)
return toJava((jint)v);
return toJava((jlong)v);
}
case NT_BOOLEAN:
return toJava((jboolean)reinterpret_cast<const QoreBoolNode *>(n)->getValue());
case NT_FLOAT:
return toJava((jdouble)reinterpret_cast<const QoreFloatNode *>(n)->f);
case NT_OBJECT: {
const QoreObject *o = reinterpret_cast<const QoreObject *>(n);
// get java object
PrivateDataRefHolder<QoreJavaPrivateData> jo(o, CID_OBJECT, xsink);
if (!jo) {
if (!*xsink)
xsink->raiseException("JAVA-UNSUPPORTED-TYPE", "cannot convert from Qore class '%s' to a Java value; '%s' does not inherit java::lang::Object", o->getClassName(), o->getClassName());
return 0;
}
return jo->getObject();
}
}
xsink->raiseException("JAVA-UNSUPPORTED-TYPE", "cannot convert from Qore '%s' to a Java value", get_type_name(n));
return 0;
}
void damage_instance::deserialize( JsonIn &jsin )
{
JsonObject jo( jsin );
// @todo Clean up
damage_units = load_damage_instance( jo ).damage_units;
}
CMUK_ERROR_CODE cmuk::computeFootIK( LegIndex leg,
const vec3f& pos,
vec3f* q_bent_forward,
vec3f* q_bent_rearward ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
} else if (!q_bent_forward || !q_bent_rearward) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
debug << "*** computing IK...\n";
int hipflags = 0;
// subtract off hip position
vec3f p = pos - jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
vec3f orig = pos;
// get dist from hip rx joint to y rotation plane
const float& d = jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK)[1];
// get the squared length of the distance on the plane
float yz = p[1]*p[1] + p[2]*p[2];
// alpha is the angle of the foot in the YZ plane with respect to the Y axis
float alpha = atan2(p[2], p[1]);
// h is the distance of foot from hip in YZ plane
float h = sqrt(yz);
// beta is the angle between the foot-hip vector (projected in YZ
// plane) and the top hip link.
float cosbeta = d / h;
debug << "p = " << p << ", d = " << d << ", yz = " << yz << "\nalpha = " << alpha << ", h = " << h << ", cosbeta=" << cosbeta << "\n";
if (fabs(cosbeta) > 1) {
debug << "violated triangle inequality when calculating hip_rx_angle!\n" ;
if (fabs(cosbeta) - 1 > 1e-4) {
hipflags = hipflags | IK_UPPER_DISTANCE;
}
cosbeta = (cosbeta < 0) ? -1 : 1;
if (yz < 1e-4) {
p[1] = d;
p[2] = 0;
} else {
float scl = fabs(d) / h;
p[1] *= scl;
p[2] *= scl;
orig = p + jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
}
}
float beta = acos(cosbeta);
// Now compute the two possible hip angles
float hip_rx_angles[2], badness[2];
int flags[2];
flags[0] = hipflags;
flags[1] = hipflags;
hip_rx_angles[0] = fix_angle(alpha - beta, -M_PI, M_PI);
hip_rx_angles[1] = fix_angle(alpha + beta, -M_PI, M_PI);
const float& min = jl(_kc, leg, HIP_RX, 0);
const float& max = jl(_kc, leg, HIP_RX, 1);
// See how badly we violate the joint limits for this hip angles
for (int i=0; i<2; ++i) {
float& angle = hip_rx_angles[i];
badness[i] = fabs(compute_badness(angle, min, max));
if (badness[i]) { flags[i] = flags[i] | IK_UPPER_ANGLE_RANGE; }
}
// Put the least bad (and smallest) hip angle first
bool swap = false;
if ( badness[1] <= badness[0] ) {
// We want the less bad solution for hip angle
swap = true;
} else if (badness[0] == 0 && badness[1] == 0) {
// We want the solution for hip angle that leaves the hip up.
if ((leg == FL || leg == HL) && hip_rx_angles[0] > hip_rx_angles[1]) {
swap = true;
} else if ((leg == FR || leg == HR) && hip_rx_angles[0] < hip_rx_angles[1]) {
swap = true;
}
}
if (swap) {
std::swap(hip_rx_angles[0], hip_rx_angles[1]);
std::swap(badness[0], badness[1]);
std::swap(flags[0], flags[1]);
}
int hip_solution_cnt = 2;
if (badness[0] == 0 && badness[1] != 0) {
hip_solution_cnt = 1;
}
debug << "hip_rx_angles[0]=" << hip_rx_angles[0]
<< ", badness=" << badness[0]
<< ", flags=" << flags[0] << "\n";
debug << "hip_rx_angles[1]=" << hip_rx_angles[1]
<< ", badness=" << badness[1]
<< ", flags=" << flags[1] << "\n";
debug << "hip_solution_cnt = " << hip_solution_cnt << "\n";
vec3f qfwd[2], qrear[2];
for (int i=0; i<hip_solution_cnt; ++i) {
debug << "** computing ll solution " << (i+1) << " of " << (hip_solution_cnt) << "\n";
float hip_rx = hip_rx_angles[i];
// now make inv. transform to get rid of hip rotation
Transform3f tx = Transform3f::rx(hip_rx, jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
vec3f ptx = tx.transformInv(orig);
debug << "tx=[" << tx.translation() << ", " << tx.rotation() << "], ptx = " << ptx << "\n";
// calculate lengths for cosine law
float l1sqr = ol2(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2sqr = ol2(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float l1 = ol(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2 = ol(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float ksqr = ptx[0]*ptx[0] + ptx[2]*ptx[2];
float k = sqrt(ksqr);
debug << "l1=" << l1 << ", l2=" << l2 << ", k=" << k << "\n";
// check triangle inequality
if (k > l1 + l2) {
debug << "oops, violated the triangle inequality for lower segments: "
<< "k = " << k << ", "
<< "l1 + l2 = " << l1 + l2 << "\n";
if (k - (l1 + l2) > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
k = l1 + l2;
ksqr = k * k;
}
// 2*theta is the acute angle formed by the spread
// of the two hip rotations...
float costheta = (l1sqr + ksqr - l2sqr) / (2 * l1 * k);
if (fabs(costheta) > 1) {
debug << "costheta = " << costheta << " > 1\n";
if (fabs(costheta) - 1 > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
costheta = (costheta < 0) ? -1 : 1;
}
float theta = acos(costheta);
// gamma is the angle of the foot with respect to the z axis
float gamma = atan2(-ptx[0], -ptx[2]);
// hip angles are just offsets off of gamma now
float hip_ry_1 = gamma - theta;
float hip_ry_2 = gamma + theta;
// phi is the obtuse angle of the parallelogram
float cosphi = (l1sqr + l2sqr - ksqr) / (2 * l1 * l2);
if (fabs(cosphi) > 1) {
debug << "cosphi = " << cosphi << " > 1\n";
if (fabs(cosphi) - 1 > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
cosphi = (cosphi < 0) ? -1 : 1;
}
float phi = acos(cosphi);
// epsilon is the "error" caused by not having feet offset directly
// along the z-axis (if they were, epsilon would equal zero)
float epsilon = le(_kc, leg, _centeredFootIK);
// now we can directly solve for knee angles
float knee_ry_1 = M_PI - phi - epsilon;
float knee_ry_2 = -M_PI + phi - epsilon;
// now fill out angle structs and check limits
qfwd[i] = vec3f(hip_rx, hip_ry_1, knee_ry_1);
qrear[i] = vec3f(hip_rx, hip_ry_2, knee_ry_2);
debug << "before wrap, qfwd = " << qfwd[i] << "\n";
debug << "before wrap, qrear = " << qrear[i] << "\n";
check_wrap(_kc, qfwd[i], leg);
check_wrap(_kc, qrear[i], leg);
debug << "after wrap, qfwd = " << qfwd[i] << "\n";
debug << "after wrap, qrear = " << qrear[i] << "\n";
if (!check_limits(_kc, qfwd[i], leg)) {
debug << "violated limits forward!\n";
flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_FWD;
}
if (!check_limits(_kc, qrear[i], leg)) {
debug << "violated limits rearward!\n";
flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_REAR;
}
} // for each viable hip solution
int best = 0;
if (hip_solution_cnt == 2) {
if (howbad(flags[0]) > howbad(flags[1])) {
best = 1;
}
debug << "best overall solution is " << (best+1) << "\n";
}
*q_bent_forward = qfwd[best];
*q_bent_rearward = qrear[best];
return flags_to_errcode(flags[best]);
}
static inline float ol2(const cmuk::KConstants& kc, int leg, int offset, bool cfoot) {
vec3f v = jo(kc, leg, offset, cfoot);
return v.norm2();
}
static inline float le(const cmuk::KConstants& kc, int leg, bool cfoot) {
vec3f v = jo(kc, leg, 3, cfoot);
return atan2(-v.x(), -v.z());
}
//---------------------------------------------------------------------------
// @function:
// CJoinOrderTest::EresUnittest_Expand
//
// @doc:
// Simple expansion test
//
//---------------------------------------------------------------------------
GPOS_RESULT
CJoinOrderTest::EresUnittest_Expand()
{
CAutoMemoryPool amp;
IMemoryPool *pmp = amp.Pmp();
// setup a file-based provider
CMDProviderMemory *pmdp = CTestUtils::m_pmdpf;
pmdp->AddRef();
CMDAccessor mda(pmp, CMDCache::Pcache(), CTestUtils::m_sysidDefault, pmdp);
// install opt context in TLS
CAutoOptCtxt aoc
(
pmp,
&mda,
NULL, /* pceeval */
CTestUtils::Pcm(pmp)
);
// build test case
CExpression *pexpr = CTestUtils::PexprLogicalNAryJoin(pmp);
DrgPexpr *pdrgpexpr = GPOS_NEW(pmp) DrgPexpr(pmp);
ULONG ulArity = pexpr->UlArity();
for (ULONG ul = 0; ul < ulArity - 1; ul++)
{
CExpression *pexprChild = (*pexpr)[ul];
pexprChild->AddRef();
pdrgpexpr->Append(pexprChild);
}
DrgPexpr *pdrgpexprConj = CPredicateUtils::PdrgpexprConjuncts(pmp, (*pexpr)[ulArity - 1]);
// add predicates selectively to trigger special case of cross join
DrgPexpr *pdrgpexprTest = GPOS_NEW(pmp) DrgPexpr(pmp);
for (ULONG ul = 0; ul < pdrgpexprConj->UlLength() - 1; ul++)
{
CExpression *pexprConjunct = (*pdrgpexprConj)[ul];
pexprConjunct->AddRef();
pdrgpexprTest->Append(pexprConjunct);
}
pdrgpexprConj->Release();
// single-table predicate
CColRefSet *pcrsOutput = CDrvdPropRelational::Pdprel((*pdrgpexpr)[ulArity - 2]->PdpDerive())->PcrsOutput();
CExpression *pexprSingleton = CUtils::PexprScalarEqCmp(pmp, pcrsOutput->PcrAny(), pcrsOutput->PcrAny());
pdrgpexprTest->Append(pexprSingleton);
CJoinOrder jo(pmp, pdrgpexpr, pdrgpexprTest);
CExpression *pexprResult = jo.PexprExpand();
{
CAutoTrace at(pmp);
at.Os() << std::endl << "INPUT:" << std::endl << *pexpr << std::endl;
at.Os() << std::endl << "OUTPUT:" << std::endl << *pexprResult << std::endl;
}
CRefCount::SafeRelease(pexprResult);
pexpr->Release();
return GPOS_OK;
}
