void uhfsolve::setupF(){
//set up the Fock matrix
double Di = 0;
double Ex = 0;
double DiMinusEx = 0;
for(int p=0;p<nStates;p++){
for(int q=0;q<nStates;q++){
//F(p,q) = Bs.h(p,q);
Fu(p,q) = Bs.h(p,q);
Fd(p,q) = Bs.h(p,q);
for(int r=0;r<nStates;r++){
for(int s=0;s<nStates;s++){
//Di = Bs.v(p,r)(q,s);
//Ex = Bs.v(p,r)(s,q);
Di = Bs.v(p,q)(r,s);
Ex = Bs.v(p,s)(r,q);
DiMinusEx = Di-Ex;
//F(p,q) += 0.5*coupledMatrixTilde(p,q,r,s)*P(r,s); //Alt. 2 27/5 2014
Fu(p,q) += Pu(s,r)*(Di-Ex) + Pd(s,r)*Di;
Fd(p,q) += Pd(s,r)*(Di-Ex) + Pu(s,r)*Di;
}
}
}
}
}
Cell* Cell::FdRight(PieceColor color) {
assert(this != NULL);
if (Fd(color) != NULL) {
return Fd(color)->Right(color);
}
else return NULL;
}
void Poll::run(int timeout_ms) {
int err = poll(pollfds_.data(), narrow_cast<int>(pollfds_.size()), timeout_ms);
auto poll_errno = errno;
LOG_IF(FATAL, err == -1 && poll_errno != EINTR) << Status::PosixError(poll_errno, "poll failed");
for (auto &pollfd : pollfds_) {
Fd::Flags flags = 0;
if (pollfd.revents & POLLIN) {
pollfd.revents &= ~POLLIN;
flags |= Fd::Read;
}
if (pollfd.revents & POLLOUT) {
pollfd.revents &= ~POLLOUT;
flags |= Fd::Write;
}
if (pollfd.revents & POLLHUP) {
pollfd.revents &= ~POLLHUP;
flags |= Fd::Close;
}
if (pollfd.revents & POLLERR) {
pollfd.revents &= ~POLLERR;
flags |= Fd::Error;
}
if (pollfd.revents & POLLNVAL) {
LOG(FATAL) << "Unexpected POLLNVAL " << tag("fd", pollfd.fd);
}
if (pollfd.revents) {
LOG(FATAL) << "Unsupported poll events: " << pollfd.revents;
}
Fd(pollfd.fd, Fd::Mode::Reference).update_flags_notify(flags);
}
}
void KQueue::run(int timeout_ms) {
timespec timeout_data;
timespec *timeout_ptr;
if (timeout_ms == -1) {
timeout_ptr = nullptr;
} else {
timeout_data.tv_sec = timeout_ms / 1000;
timeout_data.tv_nsec = timeout_ms % 1000 * 1000000;
timeout_ptr = &timeout_data;
}
int n = update(static_cast<int>(events.size()), timeout_ptr);
for (int i = 0; i < n; i++) {
struct kevent *event = &events[i];
Fd::Flags flags = 0;
if (event->filter == EVFILT_WRITE) {
flags |= Fd::Write;
}
if (event->filter == EVFILT_READ) {
flags |= Fd::Read;
}
if (event->flags & EV_EOF) {
flags |= Fd::Close;
}
if (event->fflags & EV_ERROR) {
LOG(FATAL, "EV_ERROR in kqueue is not supported");
}
VLOG(fd) << "Event [fd:" << event->ident << "] [filter:" << event->filter << "] [udata: " << event->udata << "]";
// LOG(WARNING) << "event->ident = " << event->ident << "event->filter = " << event->filter;
Fd(static_cast<int>(event->ident), Fd::Mode::Reference).update_flags_notify(flags);
}
}
static void
parse_new(struct vcc *tl)
{
struct symbol *sy1, *sy2, *sy3;
const char *p, *s_obj, *s_init, *s_struct, *s_fini;
char buf1[128];
char buf2[128];
vcc_NextToken(tl);
ExpectErr(tl, ID);
sy1 = VCC_FindSymbol(tl, tl->t, SYM_NONE);
XXXAZ(sy1);
sy1 = VCC_AddSymbolTok(tl, tl->t, SYM_NONE); // XXX: NONE ?
XXXAN(sy1);
vcc_NextToken(tl);
ExpectErr(tl, '=');
vcc_NextToken(tl);
ExpectErr(tl, ID);
sy2 = VCC_FindSymbol(tl, tl->t, SYM_OBJECT);
XXXAN(sy2);
/*lint -save -e448 */
/* Split the first three args */
p = sy2->args;
s_obj = p;
p += strlen(p) + 1;
s_init = p;
while (p[0] != '\0' || p[1] != '\0')
p++;
p += 2;
s_struct = p;
p += strlen(p) + 1;
s_fini = p + strlen(p) + 1;
while (p[0] != '\0' || p[1] != '\0')
p++;
p += 2;
Fh(tl, 0, "static %s *%s;\n\n", s_struct, sy1->name);
vcc_NextToken(tl);
bprintf(buf1, ", &%s, \"%s\"", sy1->name, sy1->name);
vcc_Eval_Func(tl, s_init, buf1, "ASDF", s_init + strlen(s_init) + 1);
Fd(tl, 0, "\t%s(&%s);\n", s_fini, sy1->name);
ExpectErr(tl, ';');
bprintf(buf1, ", %s", sy1->name);
/* Split the methods from the args */
while (*p != '\0') {
p += strlen(s_obj);
bprintf(buf2, "%s%s", sy1->name, p);
sy3 = VCC_AddSymbolStr(tl, buf2, SYM_FUNC);
AN(sy3);
sy3->eval = vcc_Eval_SymFunc;
p += strlen(p) + 1;
sy3->cfunc = p;
p += strlen(p) + 1;
/* Functions which return VOID are procedures */
if (!memcmp(p, "VOID\0", 5))
sy3->kind = SYM_PROC;
sy3->args = p;
sy3->extra = TlDup(tl, buf1);
while (p[0] != '\0' || p[1] != '\0')
p++;
p += 2;
}
/*lint -restore */
}
static void
parse_new(struct vcc *tl)
{
struct symbol *sy1, *sy2, *sy3;
const char *p, *s_obj, *s_init, *s_struct, *s_fini;
char buf1[128];
char buf2[128];
vcc_NextToken(tl);
ExpectErr(tl, ID);
if (!vcc_isCid(tl->t)) {
VSB_printf(tl->sb,
"Names of VCL objects cannot contain '-'\n");
vcc_ErrWhere(tl, tl->t);
return;
}
sy1 = VCC_FindSymbol(tl, tl->t, SYM_NONE);
XXXAZ(sy1);
sy1 = VCC_AddSymbolTok(tl, tl->t, SYM_NONE); // XXX: NONE ?
XXXAN(sy1);
vcc_NextToken(tl);
ExpectErr(tl, '=');
vcc_NextToken(tl);
ExpectErr(tl, ID);
sy2 = VCC_FindSymbol(tl, tl->t, SYM_OBJECT);
XXXAN(sy2);
/*lint -save -e448 */
/* Split the first three args */
p = sy2->args;
s_obj = p;
p += strlen(p) + 1;
s_init = p;
while (p[0] != '\0' || p[1] != '\0')
p++;
p += 2;
s_struct = p;
p += strlen(p) + 1;
s_fini = p + strlen(p) + 1;
while (p[0] != '\0' || p[1] != '\0')
p++;
p += 2;
Fh(tl, 0, "static %s *%s;\n\n", s_struct, sy1->name);
vcc_NextToken(tl);
bprintf(buf1, ", &%s, \"%s\"", sy1->name, sy1->name);
vcc_Eval_Func(tl, s_init, buf1, "ASDF", s_init + strlen(s_init) + 1);
Fd(tl, 0, "\t%s(&%s);\n", s_fini, sy1->name);
ExpectErr(tl, ';');
bprintf(buf1, ", %s", sy1->name);
/* Split the methods from the args */
while (*p != '\0') {
p += strlen(s_obj);
bprintf(buf2, "%s%s", sy1->name, p);
sy3 = VCC_AddSymbolStr(tl, buf2, SYM_FUNC);
AN(sy3);
sy3->eval = vcc_Eval_SymFunc;
p += strlen(p) + 1;
sy3->cfunc = p;
p += strlen(p) + 1;
/* Functions which return VOID are procedures */
if (!memcmp(p, "VOID\0", 5))
sy3->kind = SYM_PROC;
sy3->args = p;
sy3->extra = TlDup(tl, buf1);
while (p[0] != '\0' || p[1] != '\0') {
if (!memcmp(p, "ENUM\0", 5)) {
/* XXX: Special case for ENUM that has
it's own \0\0 end marker. Not exactly
elegant, we should consider
alternatives here. Maybe runlength
encode the entire block? */
p += strlen(p) + 1;
while (p[0] != '\0' || p[1] != '\0')
p++;
}
p++;
}
p += 2;
}
/*lint -restore */
}
//! interaction between landing surface and landing gear - accumulates a force to apply to anchor
void dynamics::LandingGear::interact(const LandingSurface &surface,
const matrix3x1 &anchor,
const matrix3x1 &anchorVelocity,
const quaternion &orientation) {
matrix3x1 up(0, 1, 0);
matrix3x1 forward(1, 0, 0);
matrix3x1 disp_hat = orientation.rotate(wheel_hat);
plane plane(surface.center, surface.orientation.rotate(up));
force = matrix3x1(0, 0, 0);
static bool firstTime = true;
float displacement = plane.intersect(anchor, disp_hat);
if (displacement > 0 && displacement < dashpot.extendedLength) {
dashpot.length = displacement;
matrix3x1 vel_t = plane.project(anchorVelocity);
matrix3x1 vel_n = anchorVelocity - vel_t;
// compute dashpot velocity
dashpot.velocity = vel_n.dot(disp_hat);
// compute forces
applied = anchor + disp_hat * displacement;
float F_dashpot = dashpot.force();
float Fn = -disp_hat.dot(plane.normal()) * F_dashpot;
matrix3x1 Fd(0, 0, 0);
float vel_t_mag = vel_t.magnitude();
if (vel_t_mag > 0.1) {
matrix3x1 vel_t_hat = vel_t / vel_t_mag;
float cos_alpha = 1;
float sin_alpha = 0;
Fd = (-Fn) * (surface.mu_0 * cos_alpha + surface.mu_1 * sin_alpha) * (vel_t_hat);
}
force += Fd;
force += (-F_dashpot) * disp_hat.dot(plane.normal()) * plane.normal();
if (firstTime) {
report("Collision!");
report(" anchor = " << anchor << ", anchorVelocity = " << anchorVelocity << ", orientation = " << orientation);
report(" displacement = " << displacement);
report(" anchorVelocity = " << anchorVelocity << ", vel_t = " << vel_t << ", vel_n = " << vel_n);
report(" vel_t_hat = " << vel_t / vel_t_mag);
report(" Fn = " << Fn << ", Fd = " << Fd);
}
firstTime = false;
}
else {
dashpot.length = dashpot.extendedLength;
dashpot.velocity = 0;
force = matrix3x1(0, 0, 0);
}
}
