Protocol* Server::request(Protocol &p) {
int dataLen = 0;
int fd = socketPool->getSocket();
if (fd < 0) {
markAsDead();
return NULL;
} else if (fd == 0) {
return NULL;
}
char *data = p.getBytes(dataLen);
int sessionId = p.getSessionID();
int err = CSocket::sendData(fd, data, dataLen, sessionId);
if (err < 0) {
socketPool->closeSocket(fd);
delete[] data;
if (!test()) {
markAsDead();
}
throw std::runtime_error("socket send data error\n");
}
socketPool->releaseSocket(fd);
delete[] data;
data = CSocket::receive(dataLen, sessionId, spp->getReceiveTimeout());
Protocol *retP = Protocol::fromBytes(data, dataLen);
if (retP == NULL) {
errno = -2;
throw std::runtime_error("Protocol is null\n");
}
if (this->state == SERVER_TESTING) {
relive();
}
free(data);
return retP;
}
void KoulesSimulator::step(const ob::State *start, const oc::Control* control,
const double t, ob::State *result)
{
unsigned int ii;
memcpy(&qcur_[0], start->as<KoulesStateSpace::StateType>()->values, numDimensions_ * sizeof(double));
time_ = 0.;
endTime_ = t;
std::fill(dead_.begin(), dead_.end(), false);
updateShip(control, t);
for (unsigned int i = 0; i <= numKoules_; ++i)
{
ii = ind(i);
dead_[i] = qcur_[ii] == -2. * kouleRadius;
if (!dead_[i])
{
if (i)
rungeKutta4(&qcur_[ii], t, &qnext_[ii]);
qcur_[ii + 2] = (qnext_[ii ] - qcur_[ii ]) / t;
qcur_[ii + 3] = (qnext_[ii + 1] - qcur_[ii + 1]) / t;
}
}
initCollisionEvents();
while (!collisionEvents_.empty())
{
CollisionEvent event = collisionEvents_.top();
double ct = std::get<0>(event);
unsigned int i = std::get<1>(event), j = std::get<2>(event);
collisionEvents_.pop();
advance(ct);
if (j <= numKoules_)
elasticCollision(i, j);
else
{
markAsDead(i);
if (i == 0)
{
memcpy(result->as<KoulesStateSpace::StateType>()->values, &qcur_[0], numDimensions_ * sizeof(double));
return;
}
}
for (unsigned int k = 0; k <= numKoules_ + 2; ++k)
{
if (k < i)
computeCollisionEvent(k, i);
else if (k > i && k != j)
computeCollisionEvent(i, k);
if (k < j && k != i)
computeCollisionEvent(k, j);
else if (k > j)
computeCollisionEvent(j, k);
}
}
advance(t);
memcpy(result->as<KoulesStateSpace::StateType>()->values, &qcur_[0], numDimensions_ * sizeof(double));
}
