// Update =====================================================
XSIPLUGINCALLBACK CStatus sn_interLocalOri_op_Update( CRef& in_ctxt )
{
OperatorContext ctxt( in_ctxt );
// Inputs
double blend = ctxt.GetParameterValue(L"blend");
KinematicState kA = (CRef)ctxt.GetInputValue(0);
KinematicState kB = (CRef)ctxt.GetInputValue(1);
KinematicState kParent = (CRef)ctxt.GetInputValue(2);
double localRotXA = ctxt.GetInputValue(3);
double localRotXB = ctxt.GetInputValue(4);
CTransformation tA = kA.GetTransform();
CTransformation tB = kB.GetTransform();
CTransformation tParent = kParent.GetTransform();
CRotation globalRotA = tA.GetRotation();
CRotation globalRotB = tB.GetRotation();
CRotation globalRotParent = tParent.GetRotation();
Application().LogMessage(L"-------------");
CVector3 vA = globalRotA.GetXYZAngles();
CVector3 vB = globalRotB.GetXYZAngles();
CVector3 vP = globalRotParent.GetXYZAngles();
Application().LogMessage(CString(vA.GetX()));
Application().LogMessage(CString(vB.GetX()));
Application().LogMessage(CString(vP.GetX()));
// Process
CVector3 v = InterpolateLocalOrientation(globalRotA, globalRotB, globalRotParent, localRotXA, localRotXB, blend);
// Get output name
CRef outputPortRef = ctxt.GetOutputPort();
OutputPort outputPort(outputPortRef);
CString outportName = outputPort.GetName();
Application().LogMessage(outportName);
Application().LogMessage(CString(v.GetX()));
Application().LogMessage(CString(v.GetY()));
Application().LogMessage(CString(v.GetZ()));
if (outportName == "rotx")
outputPort.PutValue(v.GetX());
else if (outportName == "nroty")
outputPort.PutValue(v.GetY());
else if (outportName == "nrotz")
outputPort.PutValue(v.GetZ());
else
outputPort.PutValue(0);
return CStatus::OK;
}
void AABox::Grow( const CVector3& vin )
{
mMin.SetX( CFloat::Min( mMin.GetX(), vin.GetX() ) );
mMin.SetY( CFloat::Min( mMin.GetY(), vin.GetY() ) );
mMin.SetZ( CFloat::Min( mMin.GetZ(), vin.GetZ() ) );
mMax.SetX( CFloat::Max( mMax.GetX(), vin.GetX() ) );
mMax.SetY( CFloat::Max( mMax.GetY(), vin.GetY() ) );
mMax.SetZ( CFloat::Max( mMax.GetZ(), vin.GetZ() ) );
}
bool CDynamics2DBoxEntity::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation,
bool b_check_only) {
SInt32 nCollision;
/* Check whether the box is movable or not */
if(m_cBoxEntity.GetEmbodiedEntity().IsMovable()) {
/* The box is movable */
/* Save body position and orientation */
cpVect tOldPos = m_ptBody->p;
cpFloat fOldA = m_ptBody->a;
/* Move the body to the desired position */
m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Create a shape sensor to test the movement */
/* First construct the vertices */
CVector3 cHalfSize = m_cBoxEntity.GetSize() * 0.5f;
cpVect tVertices[] = {
cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
cpv(-cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), -cHalfSize.GetY())
};
/* Then create the shape itself */
cpShape* ptTestShape = cpPolyShapeNew(m_ptBody,
4,
tVertices,
cpvzero);
/* Check if there is a collision */
nCollision = cpSpaceShapeQuery(m_cEngine.GetPhysicsSpace(), ptTestShape, NULL, NULL);
/* Dispose of the sensor shape */
cpShapeFree(ptTestShape);
if(b_check_only || nCollision) {
/* Restore old body state if there was a collision or
it was only a check for movement */
m_ptBody->p = tOldPos;
cpBodySetAngle(m_ptBody, fOldA);
}
else {
/* Update the active space hash if the movement is actual */
cpSpaceReindexShape(m_cEngine.GetPhysicsSpace(), m_ptShape);
}
}
else {
/* The box is not movable, so you can't move it :-) */
nCollision = 1;
}
/* The movement is allowed if there is no collision */
return !nCollision;
}
void CQTOpenGLUserFunctions::DrawTriangle(const CVector3& c_center_offset,
const CColor& c_color,
const bool b_fill,
const CQuaternion& c_orientation,
Real f_base,
Real f_height) {
glDisable(GL_LIGHTING);
glDisable(GL_CULL_FACE);
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
if(b_fill) {
glBegin(GL_POLYGON);
}
else {
glBegin(GL_LINE_LOOP);
}
CVector3 cNormalDirection(0.0f, 0.0f, 1.0f);
cNormalDirection.Rotate(c_orientation);
glNormal3f(cNormalDirection.GetX(),
cNormalDirection.GetY(),
cNormalDirection.GetZ());
CVector3 cVertex(f_height * 0.5f, 0.0f, 0.0f);
cVertex.Rotate(c_orientation);
glVertex3f(cVertex.GetX() + c_center_offset.GetX(),
cVertex.GetY() + c_center_offset.GetY(),
cVertex.GetZ() + c_center_offset.GetZ());
cVertex.Set(-f_height * 0.5f, f_base * 0.5f, 0.0f);
cVertex.Rotate(c_orientation);
glVertex3f(cVertex.GetX() + c_center_offset.GetX(),
cVertex.GetY() + c_center_offset.GetY(),
cVertex.GetZ() + c_center_offset.GetZ());
cVertex.Set(-f_height * 0.5f, -f_base * 0.5f, 0.0f);
cVertex.Rotate(c_orientation);
glVertex3f(cVertex.GetX() + c_center_offset.GetX(),
cVertex.GetY() + c_center_offset.GetY(),
cVertex.GetZ() + c_center_offset.GetZ());
glEnd();
glEnable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
}
void CDynamics2DMultiBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Set target position and orientation */
cpVect tBodyPos = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpFloat tBodyOrient = cZAngle.GetValue();
/* For each body: */
for(size_t i = 0; i < m_vecBodies.size(); ++i) {
/* Set body orientation at anchor */
cpBodySetAngle(m_vecBodies[i].Body,
tBodyOrient + m_vecBodies[i].OffsetOrient);
/* Set body position at anchor */
cpBodySetPos(m_vecBodies[i].Body,
cpvadd(tBodyPos,
cpvrotate(m_vecBodies[i].OffsetPos,
m_vecBodies[i].Body->rot)));
/* Update shape index */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(),
m_vecBodies[i].Body);
}
/* Update ARGoS entity state */
UpdateEntityStatus();
}
VoronoiDiagram::Point VoronoiDiagram::ToPoint(const CVector3& vec) const {
VoronoiDiagram::Point point;
/* Assuming that Vector have meter values */
point.set(HORIZONTAL, vec.GetX()*scaleVectorToMilimeters);
point.set(VERTICAL, vec.GetY()*scaleVectorToMilimeters);
return point;
}
bool CDynamics3DEntity::CheckIntersectionWithRay(Real& f_t_on_ray,
const CRay& c_ray) const {
/* Create an ODE ray from ARGoS ray */
Real fRayLength = c_ray.GetLength();
dGeomID tRay = dCreateRay(m_cEngine.GetSpaceID(), fRayLength);
CVector3 cDirection;
c_ray.GetDirection(cDirection);
dGeomRaySet(tRay,
c_ray.GetStart().GetX(),
c_ray.GetStart().GetY(),
c_ray.GetStart().GetZ(),
cDirection.GetX(),
cDirection.GetY(),
cDirection.GetZ());
/* Create the structure to contain info about the possible
ray/geom intersection */
dContactGeom tIntersection;
/* Check for intersection between the ray and the object local space */
if(dCollide(tRay, reinterpret_cast<dGeomID>(m_tEntitySpace), 1, &tIntersection, sizeof(dContactGeom)) > 0) {
/* There is an intersecton */
f_t_on_ray = tIntersection.depth / fRayLength;
return true;
}
else {
/* No intersection detected */
return false;
}
}
bool CDynamics3DEntity::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation,
bool b_check_only) {
/* Move the body to the new position */
dBodySetPosition(m_tBody, c_position.GetX(), c_position.GetY(), c_position.GetZ());
/* Rotate the body to the new orientation */
dQuaternion tQuat = { c_orientation.GetW(),
c_orientation.GetX(),
c_orientation.GetY(),
c_orientation.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
/* Check for collisions */
bool bCollisions = m_cEngine.IsEntityColliding(m_tEntitySpace);
if(bCollisions || b_check_only) {
/*
* Undo the changes if there is a collision or
* if the move was just a check
*/
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPosition.GetX(), cPosition.GetY(), cPosition.GetZ());
const CQuaternion& cOrientation = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat2 = { cOrientation.GetW(),
cOrientation.GetX(),
cOrientation.GetY(),
cOrientation.GetZ() };
dBodySetQuaternion(m_tBody, tQuat2);
return !bCollisions;
}
else {
/* Set the new position and orientation */
GetEmbodiedEntity().SetPosition(c_position);
GetEmbodiedEntity().SetOrientation(c_orientation);
return true;
}
}
void CQTOpenGLUserFunctions::DrawPolygon(const CVector3& c_position,
const CQuaternion& c_orientation,
const std::vector<CVector2>& vec_points,
const CColor& c_color,
const bool b_fill) {
if(vec_points.size() < 2) {
LOGERR << "CQTOpenGLUserFunctions::DrawPolygon() needs at least 3 points." << std::endl;
return;
}
/* Save attributes and current matrix */
glPushAttrib(GL_POLYGON_BIT);
/* Set color */
SetColor(c_color);
/* Disable face culling, to make the triangle visible from any angle */
glDisable(GL_CULL_FACE);
/* Set polygon attributes */
glEnable(GL_POLYGON_SMOOTH);
glPolygonMode(GL_FRONT_AND_BACK, b_fill ? GL_FILL : GL_LINE);
/* Set position/orientation */
Rototranslate(c_position, c_orientation);
/* Draw */
glBegin(GL_POLYGON);
glNormal3f(0.0f, 0.0f, 1.0f);
for(size_t i = 0; i < vec_points.size(); ++i) {
glVertex3f(vec_points[i].GetX(), vec_points[i].GetY(), 0.0f);
}
glEnd();
/* Reset translation */
glTranslatef(-c_position.GetX(), -c_position.GetY(), -c_position.GetZ());
/* Restore saved stuff */
glPopAttrib();
}
// Update =====================================================
XSIPLUGINCALLBACK CStatus sn_squashstretch_op_Update( CRef& in_ctxt )
{
OperatorContext ctxt( in_ctxt );
KinematicState parentKine = (CRef)ctxt.GetInputValue(0);
CTransformation parentXF = parentKine.GetTransform();
CVector3 parentScl = parentXF.GetScaling();
double blend = ctxt.GetParameterValue(L"blend");
double driver = ctxt.GetParameterValue(L"driver");
double sq_min = ctxt.GetParameterValue(L"sq_min");
double sq_max = ctxt.GetParameterValue(L"sq_max");
double st_min = ctxt.GetParameterValue(L"st_min");
double st_max = ctxt.GetParameterValue(L"st_max");
double sq_y = ctxt.GetParameterValue(L"sq_y");
double sq_z = ctxt.GetParameterValue(L"sq_z");
double st_y = ctxt.GetParameterValue(L"st_y");
double st_z = ctxt.GetParameterValue(L"st_z");
double sq_ratio = smoothstep(clamp(max(sq_max - driver,0) / max(sq_max - sq_min,0.0001),0,1));
double st_ratio = smoothstep(1.0 - clamp(max(st_max - driver,0) / max(st_max - st_min,0.0001),0,1));
sq_y *= sq_ratio;
sq_z *= sq_ratio;
st_y *= st_ratio;
st_z *= st_ratio;
parentScl.PutY(parentScl.GetY() * max( 0, 1.0 + sq_y + st_y ));
parentScl.PutZ(parentScl.GetZ() * max( 0, 1.0 + sq_z + st_z ));
parentScl.LinearlyInterpolate(parentXF.GetScaling(),parentScl,blend);
parentXF.SetScaling(parentScl);
KinematicState outKine(ctxt.GetOutputTarget());
outKine.PutTransform(parentXF);
return CStatus::OK;
}
// InterpolateLocalOrientation =============================================
CVector3 InterpolateLocalOrientation(CRotation globalRotA, CRotation globalRotB, CRotation globalRotParent, double localRotXA, double localRotXB, double blend)
{
CRotation globalRotParentInv;
globalRotParentInv.Invert(globalRotParent);
CVector3 vxA, vxB;
vxA.Set(1,0,0);
vxB.Set(1,0,0);
vxA.MulByMatrix3InPlace(globalRotA.GetMatrix());
vxB.MulByMatrix3InPlace(globalRotB.GetMatrix());
CVector3 vX, vY, vZ;
vX = InterpolateVector(vxA, vxB, blend);
vX.NormalizeInPlace();
vX.MulByMatrix3InPlace(globalRotParentInv.GetMatrix());
vZ.Set(0,0,1);
vZ.MulByMatrix3InPlace(globalRotParentInv.GetMatrix());
vY.Cross(vZ, vX);
CRotation r;
r.SetFromXYZAxes(vX, vY, vZ);
CVector3 result = r.GetXYZAngles();
result.PutX(localRotXB * blend + localRotXA * (1 - blend));
result.PutY(RadiansToDegrees(result.GetY()));
result.PutZ(RadiansToDegrees(result.GetZ()));
return result;
}
void AABox::Constrain(CVector3& test_point) const
{
if (test_point.GetX() > mMax.GetX())
test_point.SetX(mMax.GetX());
else if (test_point.GetX() < mMin.GetX())
test_point.SetX(mMin.GetX());
if (test_point.GetZ() > mMax.GetZ())
test_point.SetZ(mMax.GetZ());
else if (test_point.GetZ() < mMin.GetZ())
test_point.SetZ(mMin.GetZ());
if (test_point.GetY() > mMax.GetY())
test_point.SetY(mMax.GetY());
else if (test_point.GetY() < mMin.GetY())
test_point.SetY(mMin.GetY());
}
CVector3 rotateVectorAlongAxis(CVector3 v, CVector3 axis, double a)
{
// Angle as to be in radians
double sa = sin(a / 2.0);
double ca = cos(a / 2.0);
CQuaternion q1, q2, q2n, q;
CVector3 out;
q1.Set(0, v.GetX(), v.GetY(), v.GetZ());
q2.Set(ca, axis.GetX() * sa, axis.GetY() * sa, axis.GetZ() * sa);
q2n.Set(ca, -axis.GetX() * sa, -axis.GetY() * sa, -axis.GetZ() * sa);
q.Mul(q2, q1);
q.MulInPlace(q2n);
out.Set(q.GetX(), q.GetY(), q.GetZ());
return out;
}
buzzvm_state CBuzzController::Register(const std::string& str_key,
const CVector3& c_vec) {
buzzvm_pushs(m_tBuzzVM, buzzvm_string_register(m_tBuzzVM, str_key.c_str(), 1));
buzzvm_pusht(m_tBuzzVM);
buzzobj_t tVecTable = buzzvm_stack_at(m_tBuzzVM, 1);
buzzvm_gstore(m_tBuzzVM);
TablePut(tVecTable, "x", c_vec.GetX());
TablePut(tVecTable, "y", c_vec.GetY());
TablePut(tVecTable, "z", c_vec.GetZ());
return m_tBuzzVM->state;
}
void CQTOpenGLUserFunctions::DrawSegment(const CVector3& c_end_point,
const CVector3& c_start_point,
const CColor& c_segment_color,
const Real& f_line_width,
bool b_draw_end_point,
bool b_draw_start_point,
const CColor& c_end_point_color,
const CColor& c_start_point_color) {
/* Draw the segment */
glDisable(GL_LIGHTING);
glColor3ub(c_segment_color.GetRed(),
c_segment_color.GetGreen(),
c_segment_color.GetBlue());
glEnable (GL_LINE_SMOOTH);
glLineWidth(f_line_width);
glBegin(GL_LINES);
glVertex3f(c_start_point.GetX(),
c_start_point.GetY(),
c_start_point.GetZ() );
glVertex3f(c_end_point.GetX(),
c_end_point.GetY(),
c_end_point.GetZ() );
glEnd();
/* Draw the end and start points if necessary */
if(b_draw_end_point) {
DrawPoint(c_end_point, c_end_point_color, 5.0);
}
if(b_draw_start_point) {
DrawPoint(c_start_point, c_start_point_color, 5.0);
}
glPointSize(1.0);
glEnable(GL_LIGHTING);
}
void CDynamics2DSingleBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Move the body to the desired position */
m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Update shape index */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
/* Update ARGoS entity state */
UpdateEntityStatus();
}
static void Rototranslate(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Get Euler angles */
CRadians cZAngle, cYAngle, cXAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
/* Translate */
glTranslatef(c_position.GetX(), c_position.GetY(), c_position.GetZ());
/* Rotate */
glRotatef(ToDegrees(cXAngle).GetValue(), 1.0f, 0.0f, 0.0f);
glRotatef(ToDegrees(cYAngle).GetValue(), 0.0f, 1.0f, 0.0f);
glRotatef(ToDegrees(cZAngle).GetValue(), 0.0f, 0.0f, 1.0f);
}
buzzvm_state CBuzzController::TablePut(buzzobj_t t_table,
SInt32 n_idx,
const CVector3& c_vec) {
buzzvm_push(m_tBuzzVM, t_table);
buzzvm_pushi(m_tBuzzVM, n_idx);
buzzvm_pusht(m_tBuzzVM);
buzzobj_t tVecTable = buzzvm_stack_at(m_tBuzzVM, 1);
buzzvm_tput(m_tBuzzVM);
TablePut(tVecTable, "x", c_vec.GetX());
TablePut(tVecTable, "y", c_vec.GetY());
TablePut(tVecTable, "z", c_vec.GetZ());
return m_tBuzzVM->state;
}
bool AABox::Contains(const CVector3& test_point) const
{
if (test_point.GetX() > mMax.GetX())
return false;
if (test_point.GetX() < mMin.GetX())
return false;
if (test_point.GetZ() > mMax.GetZ())
return false;
if (test_point.GetZ() < mMin.GetZ())
return false;
if (test_point.GetY() > mMax.GetY())
return false;
if (test_point.GetY() < mMin.GetY())
return false;
return true;
}
void CFloorPowerFunctions::ResetPosition(){
CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("efootbot");
for(CSpace::TMapPerType::iterator it = m_cFootbots.begin(); it != m_cFootbots.end(); ++it) {
CEFootBotEntity& cFootBot = *any_cast<CEFootBotEntity*>(it->second);
CQuaternion qOrientation = cFootBot.GetEmbodiedEntity().GetOriginAnchor().Orientation;
CVector3 vPosition = cFootBot.GetEmbodiedEntity().GetOriginAnchor().Position;
// check up/down robots
if(vPosition.GetX() >= 7.7 || vPosition.GetX() <= -7.7){ // map edge
vPosition.SetX(vPosition.GetX() - (vPosition.GetX() * 2));
}
// check left/right robots
if(vPosition.GetY()>= 7.7 || vPosition.GetY() <= -7.7){
vPosition.SetY(vPosition.GetY() - (vPosition.GetY() * 2));
}
cFootBot.GetEmbodiedEntity().MoveTo(vPosition, qOrientation, false);
m_pcController = &dynamic_cast<CEFootBotDiffusion&>(cFootBot.GetControllableEntity().GetController());
//LOG << "Current floor color: " << m_pcFloor->GetColorAtPoint(vPosition.GetX(), vPosition.GetY())<<std::endl;
m_pcController->m_clrBottomColor = m_pcFloor->GetColorAtPoint(vPosition.GetX(), vPosition.GetY());
}
}
void CQTOpenGLUserFunctions::DrawPoint(const CVector3& c_position,
const CColor& c_color,
const Real f_point_diameter) {
glDisable(GL_LIGHTING);
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
glPointSize(f_point_diameter);
glBegin(GL_POINTS);
glVertex3f(c_position.GetX(), c_position.GetY(), c_position.GetZ());
glEnd();
glPointSize(1.0);
glEnable(GL_LIGHTING);
}
void CQTOpenGLUserFunctions::DrawText(const std::string& str_text,
const CVector3& c_center_offset,
const CColor& c_color) {
glDisable(GL_LIGHTING);
glDisable(GL_CULL_FACE);
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
GetOpenGLWidget().renderText(c_center_offset.GetX(),
c_center_offset.GetY(),
c_center_offset.GetZ(),
str_text.c_str());
glEnable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
}
void CQTOpenGLUserFunctions::DrawCircle(Real f_radius,
const CVector3& c_center_offset,
const CColor& c_color,
const bool b_fill,
const CQuaternion& c_orientation,
GLuint un_vertices) {
glDisable(GL_LIGHTING);
glDisable(GL_CULL_FACE);
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
CVector3 cVertex(f_radius, 0.0f, 0.0f);
CRadians cAngle(CRadians::TWO_PI / un_vertices);
if(b_fill) {
glBegin(GL_POLYGON);
}
else {
glBegin(GL_LINE_LOOP);
}
CVector3 cNormalDirection(0.0f, 0.0f, 1.0f);
cNormalDirection.Rotate(c_orientation);
glNormal3f(cNormalDirection.GetX(),
cNormalDirection.GetY(),
cNormalDirection.GetZ());
/* Compute the quaternion defining the rotation of the vertices used to draw the circle. */
CQuaternion cVertexRotation;
CVector3 cVertexRotationAxis(0.0f, 0.0f, 1.0f);
cVertexRotationAxis.Rotate(c_orientation);
cVertexRotation.FromAngleAxis(cAngle, cVertexRotationAxis);
cVertex.Rotate(c_orientation);
for(GLuint i = 0; i <= un_vertices; i++) {
glVertex3f(cVertex.GetX() + c_center_offset.GetX(),
cVertex.GetY() + c_center_offset.GetY(),
cVertex.GetZ() + c_center_offset.GetZ());
cVertex.Rotate(cVertexRotation);
}
glEnd();
glEnable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
}
void CQTOpenGLUserFunctions::DrawPoint(const CVector3& c_position,
const CColor& c_color,
const Real f_diameter) {
/* Save attributes */
glPushAttrib(GL_POINT_BIT);
/* Set color */
SetColor(c_color);
/* Set point attributes */
glPointSize(f_diameter);
glEnable(GL_POINT_SMOOTH);
/* Draw */
glBegin(GL_POINTS);
glVertex3f(c_position.GetX(), c_position.GetY(), c_position.GetZ());
glEnd();
/* Restore saved attributes */
glPopAttrib();
}
void CRABMedium::Init(TConfigurationNode& t_tree) {
try {
CMedium::Init(t_tree);
/* Check occlusions? */
GetNodeAttributeOrDefault(t_tree, "check_occlusions", m_bCheckOcclusions, m_bCheckOcclusions);
/* Get the positional index method */
std::string strPosIndexMethod("grid");
GetNodeAttributeOrDefault(t_tree, "index", strPosIndexMethod, strPosIndexMethod);
/* Get the arena center and size */
CVector3 cArenaCenter;
CVector3 cArenaSize;
TConfigurationNode& tArena = GetNode(CSimulator::GetInstance().GetConfigurationRoot(), "arena");
GetNodeAttribute(tArena, "size", cArenaSize);
GetNodeAttributeOrDefault(tArena, "center", cArenaCenter, cArenaCenter);
/* Create the positional index for embodied entities */
if(strPosIndexMethod == "grid") {
size_t punGridSize[3];
if(!NodeAttributeExists(t_tree, "grid_size")) {
punGridSize[0] = cArenaSize.GetX();
punGridSize[1] = cArenaSize.GetY();
punGridSize[2] = cArenaSize.GetZ();
}
else {
std::string strPosGridSize;
GetNodeAttribute(t_tree, "grid_size", strPosGridSize);
ParseValues<size_t>(strPosGridSize, 3, punGridSize, ',');
}
CGrid<CRABEquippedEntity>* pcGrid = new CGrid<CRABEquippedEntity>(
cArenaCenter - cArenaSize * 0.5f, cArenaCenter + cArenaSize * 0.5f,
punGridSize[0], punGridSize[1], punGridSize[2]);
m_pcRABEquippedEntityGridUpdateOperation = new CRABEquippedEntityGridEntityUpdater(*pcGrid);
pcGrid->SetUpdateEntityOperation(m_pcRABEquippedEntityGridUpdateOperation);
m_pcRABEquippedEntityIndex = pcGrid;
}
else {
THROW_ARGOSEXCEPTION("Unknown method \"" << strPosIndexMethod << "\" for the positional index.");
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error in initialization of the range-and-bearing medium", ex);
}
}
void CQTOpenGLUserFunctions::DrawText(const CVector3& c_position,
const std::string& str_text,
const CColor& c_color) {
/* Save attributes */
glPushAttrib(GL_ENABLE_BIT);
/* Disable lighting to make text color unaffected by light */
glDisable(GL_LIGHTING);
/* Disable culling to make text visibile from any angle */
glDisable(GL_CULL_FACE);
/* Set color */
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
GetMainWindow().
GetOpenGLWidget().
renderText(c_position.GetX(),
c_position.GetY(),
c_position.GetZ(),
str_text.c_str());
/* Restore saved attributes */
glPopAttrib();
}
bool CDynamics2DEngine::IsPointContained(const CVector3& c_point) {
/* Check that Z-coordinate is within elevation */
if(c_point.GetZ() > m_fElevation || c_point.GetZ() < m_fElevation) {
return false;
}
if(! IsEntityTransferActive()) {
/* The engine has no boundaries on XY, so the wanted point is in for sure */
return true;
}
else {
/* Check the boundaries */
for(size_t i = 0; i < m_vecSegments.size(); ++i) {
const CVector2& cP0 = m_vecSegments[i].Segment.GetStart();
const CVector2& cP1 = m_vecSegments[i].Segment.GetEnd();
Real fCriterion =
(c_point.GetY() - cP0.GetY()) * (cP1.GetX() - cP0.GetX()) -
(c_point.GetX() - cP0.GetX()) * (cP1.GetY() - cP0.GetY());
if(fCriterion > 0.0f) {
return false;
}
}
return true;
}
}
void CQTOpenGLCamera::SSettings::Translate(const CVector3& c_delta) {
Position += Forward * c_delta.GetX() + Left * c_delta.GetY() + Up * c_delta.GetZ();
Target = Position;
Target += Forward;
}
CDynamics2DBoxEntity::CDynamics2DBoxEntity(CDynamics2DEngine& c_engine,
CBoxEntity& c_entity) :
CDynamics2DEntity(c_engine, c_entity.GetEmbodiedEntity()),
m_cBoxEntity(c_entity),
m_fMass(c_entity.GetMass()),
m_ptShape(NULL),
m_ptBody(NULL) {
/* Get the size of the entity */
CVector3 cHalfSize = c_entity.GetSize() * 0.5f;
m_fHalfHeight = cHalfSize.GetZ();
/* Create a polygonal object in the physics space */
/* Start defining the vertices
NOTE: points must be defined in a clockwise winding
*/
cpVect tVertices[] = {
cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
cpv(-cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), cHalfSize.GetY()),
cpv( cHalfSize.GetX(), -cHalfSize.GetY())
};
const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
CRadians cXAngle, cYAngle, cZAngle;
GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
if(c_entity.GetEmbodiedEntity().IsMovable()) {
/* The box is movable */
/* Create the body */
m_ptBody =
cpSpaceAddBody(m_cEngine.GetPhysicsSpace(),
cpBodyNew(m_fMass,
cpMomentForPoly(m_fMass,
4,
tVertices,
cpvzero)));
m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Create the geometry */
m_ptShape =
cpSpaceAddShape(m_cEngine.GetPhysicsSpace(),
cpPolyShapeNew(m_ptBody,
4,
tVertices,
cpvzero));
/* This object is grippable */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Lots contact friction to help pushing */
m_ptShape->u = 0.7;
/* Friction with ground */
m_ptLinearFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpPivotJointNew2(m_cEngine.GetGroundBody(),
m_ptBody,
cpvzero,
cpvzero));
m_ptLinearFriction->maxBias = 0.0f; // disable joint correction
m_ptLinearFriction->maxForce = 1.49f; // emulate linear friction (this is just slightly smaller than FOOTBOT_MAX_FORCE)
m_ptAngularFriction =
cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
cpGearJointNew(m_cEngine.GetGroundBody(),
m_ptBody,
0.0f,
1.0f));
m_ptAngularFriction->maxBias = 0.0f; // disable joint correction
m_ptAngularFriction->maxForce = 1.49f; // emulate angular friction (this is just slightly smaller than FOOTBOT_MAX_TORQUE)
}
else {
/* The box is not movable */
/* Manually rotate the vertices */
cpVect tRot = cpvforangle(cZAngle.GetValue());
tVertices[0] = cpvrotate(tVertices[0], tRot);
tVertices[1] = cpvrotate(tVertices[1], tRot);
tVertices[2] = cpvrotate(tVertices[2], tRot);
tVertices[3] = cpvrotate(tVertices[3], tRot);
/* Create the geometry */
m_ptShape =
cpSpaceAddStaticShape(m_cEngine.GetPhysicsSpace(),
cpPolyShapeNew(m_cEngine.GetGroundBody(),
4,
tVertices,
cpv(cPosition.GetX(), cPosition.GetY())));
/* This object is normal */
m_ptShape->collision_type = CDynamics2DEngine::SHAPE_NORMAL;
m_ptShape->data = reinterpret_cast<void*>(&c_entity);
/* No elasticity */
m_ptShape->e = 0.0;
/* Little contact friction to help sliding away */
m_ptShape->u = 0.1;
}
}
/*****
* Required by ARGoS. This function initializes global variables using
* values from the XML configuration file supplied when ARGoS is run.
*****/
void DSA_loop_functions::Init(TConfigurationNode& node) {
/* Temporary variables. */
CSimulator *simulator = &GetSimulator();
CPhysicsEngine *physicsEngine = &simulator->GetPhysicsEngine("default");
CVector3 ArenaSize = GetSpace().GetArenaSize();
CVector2 rangeX = CVector2(-ArenaSize.GetX()/2.0, ArenaSize.GetX()/2.0);
CVector2 rangeY = CVector2(-ArenaSize.GetY()/2.0, ArenaSize.GetY()/2.0);
CDegrees USV_InDegrees;
/* Get each tag in the loop functions section of the XML file. */
TConfigurationNode simNode = GetNode(node, "simulation");
TConfigurationNode random = GetNode(node, "_0_FoodDistribution_Random");
TConfigurationNode cluster = GetNode(node, "_1_FoodDistribution_Cluster");
TConfigurationNode powerLaw = GetNode(node, "_2_FoodDistribution_PowerLaw");
GetNodeAttribute(simNode, "MaxSimCounter", MaxSimCounter);
GetNodeAttribute(simNode, "VariableSeed", VariableSeed);
GetNodeAttribute(simNode, "OutputData", OutputData);
GetNodeAttribute(simNode, "NestPosition", NestPosition);
GetNodeAttribute(simNode, "NestRadius", NestRadius);
GetNodeAttribute(simNode, "FoodRadius", FoodRadius);
GetNodeAttribute(simNode, "FoodDistribution", FoodDistribution);
GetNodeAttribute(random, "FoodItemCount", FoodItemCount);
GetNodeAttribute(cluster, "NumberOfClusters", NumberOfClusters);
GetNodeAttribute(cluster, "ClusterWidthX", ClusterWidthX);
GetNodeAttribute(cluster, "ClusterLengthY", ClusterLengthY);
GetNodeAttribute(powerLaw, "PowerRank", PowerRank);
/* Convert and calculate additional values. */
NestRadiusSquared = (NestRadius) * (NestRadius);
FoodRadiusSquared = (FoodRadius + 0.04) * (FoodRadius + 0.04);
ForageRangeX.Set(rangeX.GetX() + (2.0 * FoodRadius),
rangeX.GetY() - (2.0 * FoodRadius));
ForageRangeY.Set(rangeY.GetX() + (2.0 * FoodRadius),
rangeY.GetY() - (2.0 * FoodRadius));
RNG = CRandom::CreateRNG("argos");
/* Store the iAnts in a more friendly, human-readable structure. */
CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot");
CSpace::TMapPerType::iterator it;
ReadFile();
size_t STOP = 0;
size_t robots = 0;
for(it = footbots.begin(); it != footbots.end(); it++)
{
CFootBotEntity& footBot = *any_cast<CFootBotEntity*>(it->second);
DSA_controller& c = dynamic_cast<DSA_controller&>(footBot.GetControllableEntity().GetController());
DSAnts.push_back(&c);
c.SetData(&data);
c.SetStop(STOP);
STOP += 10; /* adds 10 seconds extra pause for each robot */
if(robots <= N_robots)
{
c.GetPattern(robotPattern[robots]);
robots++;
//attempt to add mutiple colors for each robot
// if(robots==2)
// {
// DrawTargetRays = 2;
// }
// else if(robots ==3)
// {
// DrawTargetRays = 3;
// }
}
}
/* Set up the food distribution based on the XML file. */
SetFoodDistribution();
}
