// 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(); }