//----------------------------------------------------------------------
// univariate slice sampling to set each element
void CS::draw_one() {
double oldr = R_(i_, j_);
double logp_star = logp(R_(i_, j_));
double u = logp_star - rexp_mt(rng(), 1);
find_limits();
if (lo_ >= hi_) {
set_r(0);
return;
}
// const double eps(100*std::numeric_limits<double>::epsilon());
const double eps(1e-6);
check_limits(oldr, eps);
while (1) {
double cand = runif_mt(rng(), lo_, hi_);
double logp_cand = logp(cand);
if (logp_cand > u) { // found something inside slice
set_r(cand);
return;
} else { // contract slice
if (cand > oldr) {
hi_ = cand;
} else {
lo_ = cand;
}
}
if (fabs(hi_ - lo_) < eps) {
set_r(hi_);
return;
}
}
}
void
init_connection(struct vsf_session* p_sess)
{
if (tunable_setproctitle_enable)
{
vsf_sysutil_setproctitle("not logged in");
}
/* Before we talk to the remote, make sure an alarm is set up in case
* writing the initial greetings should block.
*/
vsf_cmdio_set_alarm(p_sess);
/* Check limits before doing an implicit SSL handshake, to avoid DoS
* attacks. This will result in plain text messages being sent to the SSL
* client, but we can live with that.
*/
check_limits(p_sess);
if (tunable_ssl_enable && tunable_implicit_ssl)
{
ssl_control_handshake(p_sess);
}
if (tunable_ftp_enable)
{
emit_greeting(p_sess);
}
parse_username_password(p_sess);
}
static void
rotate(void)
{
/*
* It the old rotation is the same nothing has to be done.
*/
if (_rot_cities != NULL && _rotation == rotation)
return;
/*
* Free and allocate memory for the cities in the rotated plane.
*/
if (_rot_cities != NULL)
free(_rot_cities);
if ((_rot_cities = calloc(tsp->dimension, sizeof(City))) == NULL)
errx(EX_OSERR, "No memory left!");
/*
* Rotate the cities.
*/
for (int i = 0; i < tsp->dimension; i++) {
_rot_cities[i].x = tsp->cities[i].x * cos(rotation) +
tsp->cities[i].y * sin(rotation);
_rot_cities[i].y = -tsp->cities[i].x * sin(rotation) +
tsp->cities[i].y * cos(rotation);
}
/*
* Update the cached cities.
*/
_rotation = rotation;
_num_cities = tsp->dimension;
check_limits();
}
ushort check_params(t_params *params)
{
if (!check_limits(params->width, 1, 0) &&
!check_limits(params->height, 1, 0))
if (!check_limits(params->time, 1, 1000000))
if (!check_teams_reserved(params->teams))
if (!check_teams_minimum(params->teams))
if (!check_teams_uniqueness(params->teams))
if (!check_limits(params->maxclient, 1, 0))
return (0);
else
return (_xw(ERR_CLIENT));
else
return (_xw(ERR_TEAMS));
else
return (_xw(ERR_TEAMS_MIN));
else
return (_xw(ERR_TEAMS_RSV));
else
return (_xw(ERR_TIME));
else
return (_xw(ERR_MAP));
}
inline void G1CMTask::process_grey_object(oop obj) {
assert(scan || obj->is_typeArray(), "Skipping scan of grey non-typeArray");
assert(G1CMObjArrayProcessor::is_array_slice(obj) || _nextMarkBitMap->isMarked((HeapWord*) obj),
"Any stolen object should be a slice or marked");
if (scan) {
if (G1CMObjArrayProcessor::is_array_slice(obj)) {
_words_scanned += _objArray_processor.process_slice(obj);
} else if (G1CMObjArrayProcessor::should_be_sliced(obj)) {
_words_scanned += _objArray_processor.process_obj(obj);
} else {
_words_scanned += obj->oop_iterate_size(_cm_oop_closure);;
}
}
check_limits();
}
static void
process_rules(char *line)
{
time_t tstamp;
ListItem *item;
struct pattern_rule *rule;
regmatch_t parens[REGEX_PARENS_SIZE];
long month, day, hour, minute, second;
(void) convertSyslog(line, &month, &day, &hour, &minute, &second, NULL);
convertToGmt(assumed_year, month, day, hour, minute, second, assumed_tz, &tstamp);
for (item = pattern_rules.head; item != NULL; item = item->next) {
rule = item->data;
if (regexec(&rule->re, line, REGEX_PARENS_SIZE, parens, 0) == 0) {
(void) append_log(line, rule, parens, tstamp);
if (rule->limits[0] != NULL)
check_limits(line, rule, parens, tstamp);
if (first_match_only)
break;
}
}
}
/*
* If user is in the connecting state, we need to do fairly
* strict checking of all arguments.
* This means we disconnect users when they provide invalid data
* during the login sequence.
* When users are merely updating their data after successful login
* we can just ignore any invalid data and not broadcast it.
*
* The data we need to check is:
* - nick name (valid, not taken, etc)
* - CID/PID (valid, not taken, etc).
* - IP addresses (IPv4 and IPv6)
*/
int hub_handle_info(struct hub_info* hub, struct hub_user* user, const struct adc_message* cmd_unmodified)
{
int ret;
struct adc_message* cmd = adc_msg_copy(cmd_unmodified);
if (!cmd) return -1; /* OOM */
cmd->priority = 1;
hub_handle_info_common(user, cmd);
/* If user is logging in, perform more checks,
otherwise only a few things need to be checked.
*/
if (user_is_connecting(user))
{
/*
* Don't allow the user to send multiple INF messages in this stage!
* Since that can have serious side-effects.
*/
if (user->info)
{
adc_msg_free(cmd);
return 0;
}
ret = hub_handle_info_login(hub, user, cmd);
if (ret < 0)
{
on_login_failure(hub, user, ret);
adc_msg_free(cmd);
return -1;
}
else
{
/* Post a message, the user has joined */
struct event_data post;
memset(&post, 0, sizeof(post));
post.id = UHUB_EVENT_USER_JOIN;
post.ptr = user;
post.flags = ret; /* 0 - all OK, 1 - need authentication */
event_queue_post(hub->queue, &post);
adc_msg_free(cmd);
return 0;
}
}
else
{
/* These must not be allowed updated, let's remove them! */
adc_msg_remove_named_argument(cmd, ADC_INF_FLAG_PRIVATE_ID);
adc_msg_remove_named_argument(cmd, ADC_INF_FLAG_CLIENT_ID);
/*
* If the nick is not accepted, do not relay it.
* Otherwise, the nickname will be updated.
*/
if (adc_msg_has_named_argument(cmd, ADC_INF_FLAG_NICK))
{
#ifdef ALLOW_CHANGE_NICK
if (!check_nick(hub, user, cmd))
#endif
adc_msg_remove_named_argument(cmd, ADC_INF_FLAG_NICK);
}
ret = check_limits(hub, user, cmd);
if (ret < 0)
{
on_update_failure(hub, user, ret);
adc_msg_free(cmd);
return -1;
}
strip_network(user, cmd);
hub_handle_info_low_bandwidth(hub, user, cmd);
user_update_info(user, cmd);
if (!adc_msg_is_empty(cmd))
{
route_message(hub, user, cmd);
}
adc_msg_free(cmd);
}
return 0;
}
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]);
}
vec3f cmuk::clampToLimits(cmuk::LegIndex leg, const vec3f& q) const {
vec3f rval = q;
check_limits(_kc, rval, leg);
return rval;
}
