TEST(Physics, moveEntity) {
PhysicsEngine phyEg;
Vector2 initialPos(3.0f, 3.0f);
Vector2 newPos(5.0f, 3.0f);
Rectangle2 box(0.5f, 0.5f);
auto ent = std::unique_ptr < TestEntity > (new TestEntity(initialPos, box));
ent->setCollisionGroup(2);
// collide with the floor and other from our collision group
ent->setCollisionMask(CollisionGroups::Ground | 2);
ent->setCollisionType(CollisionType::CircleDynamic);
phyEg.registerEntity(ent.get());
ent->setMoveIntent(newPos);
// run for one second
// dont make this time to big, as bullet can only subdivide in a certain part of substeps
phyEg.step(0.1f);
ASSERT_FLOAT_EQ(ent->getMoveIntent().x(), newPos.x());
ASSERT_FLOAT_EQ(ent->getMoveIntent().y(), newPos.y());
ASSERT_EQ(phyEg.getRegisteredEntitiesCount(), size_t(1));
}
//! Clears the complex on the specified region. Please observe that the actually cleared region
//! consists of all lattice cells intersecting the passed region, therefore resulting in a cleared region
//! typically larger than passed one, up to the lattice granularity.
void TCacheResource::clear(QRegion region)
{
if (!m_region.intersects(region))
return;
//Get the region bbox
TRect bbox(toTRect(region.boundingRect()));
//For all cells intersecting the bbox
TPoint initialPos(getCellPos(bbox.getP00()));
TPoint pos;
for (pos.x = initialPos.x; pos.x <= bbox.x1; pos.x += latticeStep)
for (pos.y = initialPos.y; pos.y <= bbox.y1; pos.y += latticeStep) {
QRect cellQRect(toQRect(TRect(pos, TDimension(latticeStep, latticeStep))));
if (region.intersects(cellQRect) && m_region.intersects(cellQRect)) {
//Release the associated cell from cache and clear the cell from the content region.
TImageCache::instance()->remove(getCellCacheId(pos));
m_region -= cellQRect;
--m_cellsCount;
//DIAGNOSTICS_GLOADD("crCellsCnt", -1);
//Release the cell from m_cellDatas
m_cellDatas[getCellIndex(pos)].m_modified = true;
}
}
if (m_region.isEmpty()) {
m_tileType = NONE;
m_locksCount = 0;
}
}
TEST(Physics, collideEntityWithEntity) {
PhysicsEngine phyEg;
Vector2 initialPos(3.0f, 3.0f);
Vector2 newPos(3.0f, 5.0f);
Rectangle2 box(0.5f, 0.5f);
auto ent = std::unique_ptr < TestEntity > (new TestEntity(initialPos, box));
ent->setCollisionGroup(2);
// collide with the floor and other from our collision group
ent->setCollisionMask(CollisionGroups::Ground | 2);
ent->setCollisionType(CollisionType::CircleDynamic);
phyEg.registerEntity(ent.get());
ent->setMoveIntent(newPos);
// circle in between
Rectangle2 circleCollide(1.0f, 1.0f);
Vector2 wallPos(3.0f, 4.0f);
auto entWall = std::unique_ptr < TestEntity > (new TestEntity(wallPos, circleCollide));
entWall->setCollisionGroup(2);
// collide with the floor and other from our collision group
entWall->setCollisionMask(CollisionGroups::Ground | 2);
entWall->setCollisionType(CollisionType::CircleDynamic);
phyEg.registerEntity(entWall.get());
//entWall->setMoveIntent(newPos);
// run for one second
// dont make this time to big, as bullet can only subdivide in a certain part of substeps
phyEg.step(0.1f);
std::cout << "is at " << ent->getMoveIntent() << std::endl;
ASSERT_TRUE(ent->getMoveIntent().y() < newPos.y());
}
void UBGraphicsVideoItem::hasVideoChanged(bool hasVideo)
{
if(hasVideo && mMediaObject->isSeekable())
{
hasMediaChanged(hasVideo);
UBGraphicsVideoItemDelegate *vid = dynamic_cast<UBGraphicsVideoItemDelegate *>(mDelegate);
if (vid)
vid->updateTicker(initialPos());
}
}
void UBGraphicsMediaItem::hasMediaChanged(bool hasMedia)
{
if(hasMedia && mMediaObject->isSeekable())
{
Q_UNUSED(hasMedia);
mMediaObject->seek(mInitialPos);
UBGraphicsMediaItemDelegate *med = dynamic_cast<UBGraphicsMediaItemDelegate *>(Delegate());
if (med)
med->updateTicker(initialPos());
}
}
void TCacheResource::addRef2(const TRect &rect)
{
//DIAGNOSTICS_NUMBEREDSTRSET(prefix + QString::number((UINT) this) + " | Stack | ",
//"crStack", "addRef", ::traduce(rect));
//Add a reference to all cells intersecting the passed one
TPoint initialPos(getCellPos(rect.getP00()));
TPoint pos;
for (pos.x = initialPos.x; pos.x <= rect.x1; pos.x += latticeStep)
for (pos.y = initialPos.y; pos.y <= rect.y1; pos.y += latticeStep) {
PointLess cellIndex(getCellIndex(pos));
CellData &cellData = m_cellDatas[cellIndex];
cellData.m_referenced = true;
cellData.m_refsCount++;
}
}
//! Copies the passed tile in the tile complex. The passed tile \b must
//! possess integer geometry (ie tile.m_pos must have integer coordinates),
//! otherwise this function is a no-op.
bool TCacheResource::upload(const TPoint &pos, TRasterP ras)
{
int tileType;
if (!checkRasterType(ras, tileType))
return false;
if (m_tileType == NONE)
m_tileType = tileType;
//For all cells of the lattice which intersect the tile, upload the content in the
//complex
TRect tileRect(ras->getBounds() + pos);
TPoint initialPos(getCellPos(tileRect.getP00()));
//DIAGNOSTICS_NUMBEREDSTRSET(prefix + QString::number((UINT) this) + " | Stack | ",
//"crStack", "upload", ::traduce(TRect(pos, ras->getSize())));
TPoint currPos;
for (currPos.x = initialPos.x; currPos.x <= tileRect.x1; currPos.x += latticeStep)
for (currPos.y = initialPos.y; currPos.y <= tileRect.y1; currPos.y += latticeStep) {
//Copy tile's content into the cell's raster.
TRect cellRect(currPos, TDimension(latticeStep, latticeStep));
TRect overlapRect(tileRect * cellRect);
assert(!overlapRect.isEmpty());
PointLess cellIndex(getCellIndex(currPos));
std::pair<TRasterP, CellData *> cellInfos(touch(cellIndex));
TRasterP cellRas(cellInfos.first);
TRect temp(overlapRect - currPos);
TRasterP overlappingCellRas(cellRas->extract(temp));
temp = TRect(overlapRect - tileRect.getP00());
TRasterP overlappingTileRas(ras->extract(temp));
assert(overlappingCellRas->getBounds() == overlappingTileRas->getBounds());
TRop::copy(overlappingCellRas, overlappingTileRas);
cellInfos.second->m_modified = true;
}
//Update the complex's content region
m_region += toQRect(tileRect);
return true;
}
bool TCacheResource::downloadAll(const TPoint &pos, TRasterP ras)
{
int tileType;
if (!checkRasterType(ras, tileType))
return false;
//Build the tile's rect
TRect tileRect(ras->getBounds() + pos);
if (!contains(m_region, tileRect))
return false;
//DIAGNOSTICS_NUMBEREDSTRSET(prefix + QString::number((UINT) this) + " | Stack | ",
//"crStack", "downloadAll", ::traduce(TRect(pos, ras->getSize())));
//For all cells intersecting the tile's rect, copy all those intersecting the
//complex's content region.
TPoint initialPos(getCellPos(tileRect.getP00()));
TPoint currPos;
for (currPos.x = initialPos.x; currPos.x <= tileRect.x1; currPos.x += latticeStep)
for (currPos.y = initialPos.y; currPos.y <= tileRect.y1; currPos.y += latticeStep) {
TRect cellRect(currPos, TDimension(latticeStep, latticeStep));
TRect overlapRect(tileRect * cellRect);
assert(!overlapRect.isEmpty());
QRect overlapQRect(toQRect(overlapRect));
if (m_region.intersects(overlapQRect)) {
//Extract the associated rasters and perform the copy to the input tile.
std::pair<TRasterP, CellData *> cellInfos(touch(getCellIndex(currPos)));
TRasterP cellRas(cellInfos.first);
TRect temp(overlapRect - currPos);
TRasterP overlappingCellRas(cellRas->extract(temp));
temp = TRect(overlapRect - tileRect.getP00());
TRasterP overlappingTileRas(ras->extract(temp));
TRop::copy(overlappingTileRas, overlappingCellRas);
}
}
return true;
}
//! Fills the passed tile with the data contained in the complex, returning
//! the copied region.
//! The same restriction of the upload() method applies here.
QRegion TCacheResource::download(const TPoint &pos, TRasterP ras)
{
int tileType;
if (!checkRasterType(ras, tileType))
return QRegion();
//Build the tile's rect
TRect tileRect(ras->getBounds() + pos);
if (!m_region.intersects(toQRect(tileRect)))
return QRegion();
//For all cells intersecting the tile's rect, copy all those intersecting the
//complex's content region.
TPoint initialPos(getCellPos(tileRect.getP00()));
TPoint currPos;
for (currPos.x = initialPos.x; currPos.x <= tileRect.x1; currPos.x += latticeStep)
for (currPos.y = initialPos.y; currPos.y <= tileRect.y1; currPos.y += latticeStep) {
TRect cellRect(currPos, TDimension(latticeStep, latticeStep));
TRect overlapRect(tileRect * cellRect);
assert(!overlapRect.isEmpty());
QRect overlapQRect(toQRect(overlapRect));
if (m_region.intersects(overlapQRect)) {
//Extract the associated rasters and perform the copy to the input tile.
std::pair<TRasterP, CellData *> cellInfos(touch(getCellIndex(currPos)));
TRasterP cellRas(cellInfos.first);
TRect temp(overlapRect - currPos);
TRasterP overlappingCellRas(cellRas->extract(temp));
temp = TRect(overlapRect - tileRect.getP00());
TRasterP overlappingTileRas(ras->extract(temp));
TRop::copy(overlappingTileRas, overlappingCellRas);
}
}
return m_region.intersected(QRegion(toQRect(tileRect)));
}
void ParticleSystem::activate(Particle* p)
{
double ttl = m_ttl.random();
Vector3 acc = m_accelVector * m_accelMag.random();
Color_4d initialColor = interpolate(twister.rand(), m_initialColors[0], m_initialColors[1]);
Color_4d finalColor = interpolate(twister.rand(), m_finalColors[0], m_finalColors[1]);
initialColor.a = m_initialAlpha.random();
finalColor.a = m_finalAlpha.random();
double initialSize = m_initialSize.random();
p->init(ttl, initialPos(), initialVelocity(), acc,
initialSize,
(m_finalSize.random() - initialSize) / ttl,
initialColor,
(finalColor - initialColor) * (1.0 / ttl),
m_emitSpinSpeed.random(), 0.0
);
}
void PhysicsServerSharedMemory::processClientCommands()
{
if (m_data->m_isConnected && m_data->m_testBlock1)
{
///we ignore overflow of integer for now
if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands)
{
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1);
const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
m_data->m_testBlock1->m_numProcessedClientCommands++;
//no timestamp yet
int timeStamp = 0;
//consume the command
switch (clientCmd.m_type)
{
case CMD_SEND_BULLET_DATA_STREAM:
{
b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
if (completedOk)
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
m_data->submitServerStatus(status);
} else
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
}
break;
}
case CMD_LOAD_URDF:
{
//at the moment, we only load 1 urdf / robot
if (m_data->m_urdfLinkNameMapper.size())
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
break;
}
const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName);
btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0);
btAssert(urdfArgs.m_urdfFileName);
btVector3 initialPos(0,0,0);
btQuaternion initialOrn(0,0,0,1);
if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION)
{
initialPos[0] = urdfArgs.m_initialPosition[0];
initialPos[1] = urdfArgs.m_initialPosition[1];
initialPos[2] = urdfArgs.m_initialPosition[2];
}
if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION)
{
initialOrn[0] = urdfArgs.m_initialOrientation[0];
initialOrn[1] = urdfArgs.m_initialOrientation[1];
initialOrn[2] = urdfArgs.m_initialOrientation[2];
initialOrn[3] = urdfArgs.m_initialOrientation[3];
}
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? urdfArgs.m_useMultiBody : true;
bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? urdfArgs.m_useFixedBase: false;
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
initialPos,initialOrn,
useMultiBody, useFixedBase);
SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
if (completedOk)
{
if (m_data->m_urdfLinkNameMapper.size())
{
serverCmd.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
} else
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
}
break;
}
case CMD_CREATE_SENSOR:
{
b3Printf("Processed CMD_CREATE_SENSOR");
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
btAssert(mb);
for (int i=0;i<clientCmd.m_createSensorArguments.m_numJointSensorChanges;i++)
{
int jointIndex = clientCmd.m_createSensorArguments.m_jointIndex[i];
if (clientCmd.m_createSensorArguments.m_enableJointForceSensor[i])
{
if (mb->getLink(jointIndex).m_jointFeedback)
{
b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex);
} else
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
fb->m_reactionForces.setZero();
mb->getLink(jointIndex).m_jointFeedback = fb;
m_data->m_multiBodyJointFeedbacks.push_back(fb);
};
} else
{
if (mb->getLink(jointIndex).m_jointFeedback)
{
m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback);
delete mb->getLink(jointIndex).m_jointFeedback;
mb->getLink(jointIndex).m_jointFeedback=0;
} else
{
b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex);
};
}
}
} else
{
b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet");
}
#if 0
//todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody
/*
for (int i=0;i<m_data->m_dynamicsWorld->getNumConstraints();i++)
{
btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i);
btJointFeedback* fb = new btJointFeedback();
m_data->m_jointFeedbacks.push_back(fb);
c->setJointFeedback(fb);
}
*/
#endif
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_SEND_DESIRED_STATE:
{
b3Printf("Processed CMD_SEND_DESIRED_STATE");
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
btAssert(mb);
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
{
case CONTROL_MODE_TORQUE:
{
b3Printf("Using CONTROL_MODE_TORQUE");
mb->clearForcesAndTorques();
int torqueIndex = 0;
btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
for (int link=0;link<mb->getNumLinks();link++)
{
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{
double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque);
torqueIndex++;
}
}
break;
}
case CONTROL_MODE_VELOCITY:
{
b3Printf("Using CONTROL_MODE_VELOCITY");
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link=0;link<mb->getNumLinks();link++)
{
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link];
if (motorPtr)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}
}
dofIndex += mb->getLink(link).m_dofCount;
}
}
break;
}
default:
{
b3Printf("m_controlMode not implemented yet");
break;
}
}
}
SharedMemoryStatus& status = m_data->createServerStatus(CMD_DESIRED_STATE_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
b3Printf("Sending the actual state (Q,U)");
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
//always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
{
btTransform tr;
tr.setOrigin(mb->getBasePos());
tr.setRotation(mb->getWorldToBaseRot().inverse());
//base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
}
for (int l=0;l<mb->getNumLinks();l++)
{
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
{
serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
}
for (int d=0;d<mb->getLink(l).m_dofCount;d++)
{
serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
}
if (0 == mb->getLink(l).m_jointFeedback)
{
for (int d=0;d<6;d++)
{
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0;
}
} else
{
btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2];
}
}
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
m_data->submitServerStatus(serverCmd);
} else
{
b3Warning("Request state but no multibody available");
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
}
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
b3Printf("Step simulation request");
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_STEP_FORWARD_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY)
{
btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0],
clientCmd.m_physSimParamArgs.m_gravityAcceleration[1],
clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]);
this->m_data->m_dynamicsWorld->setGravity(grav);
b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]);
}
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
};
case CMD_INIT_POSE:
{
b3Printf("Server Init Pose not implemented yet");
///@todo: implement this
m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0));
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_RESET_SIMULATION:
{
//clean up all data
m_data->m_guiHelper->getRenderInterface()->removeAllInstances();
deleteDynamicsWorld();
createEmptyDynamicsWorld();
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
btVector3 halfExtents(1,1,1);
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS)
{
halfExtents = btVector3(
clientCmd.m_createBoxShapeArguments.m_halfExtentsX,
clientCmd.m_createBoxShapeArguments.m_halfExtentsY,
clientCmd.m_createBoxShapeArguments.m_halfExtentsZ);
}
btTransform startTrans;
startTrans.setIdentity();
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION)
{
startTrans.setOrigin(btVector3(
clientCmd.m_createBoxShapeArguments.m_initialPosition[0],
clientCmd.m_createBoxShapeArguments.m_initialPosition[1],
clientCmd.m_createBoxShapeArguments.m_initialPosition[2]));
}
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION)
{
b3Printf("test\n");
startTrans.setRotation(btQuaternion(
clientCmd.m_createBoxShapeArguments.m_initialOrientation[0],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[1],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[2],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[3]));
}
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
btCollisionShape* shape = worldImporter->createBoxShape(halfExtents);
btScalar mass = 0.f;
bool isDynamic = (mass>0);
worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
default:
{
b3Error("Unknown command encountered");
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_UNKNOWN_COMMAND_FLUSHED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
}
};
}
}
}
void TestFindQuestionableNode() {
COsGame game;
game.Initialize("8");
CQPosition initialPos(game, 0);
CQPosition questionablePosition;
CBook book;
bool drawToMover;
// The position is not in book. FindQuestionableNode should return NULL.
bool isQuestionable = book.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(!isQuestionable);
// add the initial position to the book. FindQuestionableNode should return the initial position.
const CBitBoard initialBitBoard(initialPos.BitBoard());
CMinimalReflection mr(initialBitBoard);
CHeightInfoX heightInfoX(2, 4, false, initialBitBoard.NEmpty());
book.StoreLeaf(CMinimalReflection(initialBitBoard), heightInfoX, 0);
isQuestionable = book.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(isQuestionable);
TEST(questionablePosition==initialPos);
// add the initial position to the book with a deviation of the deviation cutoff
// Since this is bigger than the threshold for deviations, we won't count it.
book.StoreLeaf(CMinimalReflection(initialBitBoard), heightInfoX, deviationCutoff);
isQuestionable = book.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(!isQuestionable);
// but if it's one less than the deviation cutoff, it's questionable
book.StoreLeaf(CMinimalReflection(initialBitBoard), heightInfoX, deviationCutoff-1);
isQuestionable = book.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(isQuestionable);
TEST(questionablePosition==initialPos);
// Also if it's negative... at the deviation cutoff is ok, one more is questionable
book.StoreLeaf(CMinimalReflection(initialBitBoard), heightInfoX, -deviationCutoff);
isQuestionable = book.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(!isQuestionable);
book.StoreLeaf(CMinimalReflection(initialBitBoard), heightInfoX, 1-deviationCutoff);
isQuestionable = book.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(isQuestionable);
TEST(questionablePosition==initialPos);
// This node is a draw, at +3/-3.. so it's ok
const CBookData* bookData = book.FindData(initialBitBoard);
// casting to remove const is a really bad idea... but no other easy way to test
CBookValue& bookValue = (CBookValue&)(bookData->Values());
bookValue.vHeuristic = 0;
bookValue.vMover = drawCutoff;
bookValue.vOpponent = - drawCutoff;
isQuestionable = book.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(!isQuestionable);
TEST(questionablePosition==initialPos);
CQPosition nextPos(initialPos);
nextPos.MakeMove(CMove("F5"));
const CBitBoard f5 = nextPos.BitBoard();
nextPos.MakeMove(CMove("D6"));
const CBitBoard f5d6 = nextPos.BitBoard();
nextPos = initialPos;
nextPos.MakeMove(CMove("F5"));
nextPos.MakeMove(CMove("F6"));
const CBitBoard f5f6 = nextPos.BitBoard();
// Now we have a questionable position, but it's not at the initial node
// tree is:
// f5 d6 : proven draw
// f5 f6 : deviation with value drawValue
book.StoreRoot(initialBitBoard, heightInfoX, 0, -16400);
book.StoreRoot(f5, heightInfoX, 0, -16400);
CHeightInfoX hixSolved = CHeightInfoX(f5f6.NEmpty(), 0, false, f5f6.NEmpty());
book.StoreLeaf(f5f6, hixSolved, 0);
book.StoreLeaf(f5d6, heightInfoX, drawCutoff);
book.NegamaxAll();
isQuestionable = book.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(isQuestionable);
TEST(CMinimalReflection(questionablePosition.BitBoard())==CMinimalReflection(f5d6));
// This position has a questionable node, but it's not on the draw tree so it's not returned
CBook book2;
book2.StoreRoot(initialBitBoard, heightInfoX, -deviationCutoff, -16400);
book2.StoreRoot(f5, heightInfoX, deviationCutoff, -16400);
book2.StoreLeaf(f5f6, heightInfoX, -deviationCutoff);
book2.StoreLeaf(f5d6, heightInfoX, 0);
book2.NegamaxAll();
isQuestionable = book2.FindQuestionableNode(questionablePosition, drawToMover, initialPos, drawCutoff, deviationCutoff);
TEST(!isQuestionable);
}
