void CEmbodiedEntity::Init(TConfigurationNode& t_tree) {
try {
/* Initialize base entity */
CEntity::Init(t_tree);
/* Get the position of the entity */
GetNodeAttributeOrDefault(t_tree, "position", m_cInitOriginPosition, CVector3());
/* Get the orientation of the entity */
GetNodeAttributeOrDefault(t_tree, "orientation", m_cInitOriginOrientation, CQuaternion());
/* Create origin anchor */
m_psOriginAnchor = new SAnchor("origin",
0,
CVector3(),
CQuaternion(),
m_cInitOriginPosition,
m_cInitOriginOrientation);
/* Add anchor to map and enable it */
m_mapAnchors[m_psOriginAnchor->Id] = m_psOriginAnchor;
EnableAnchor("origin");
/* Embodied entities are movable by default */
m_bMovable = true;
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize embodied entity \"" << GetContext() << GetId() << "\".", ex);
}
}
CYukawaDipolePt CBaseSimCtrl::getPt(size_t i) {
CYukawaDipolePt pt(SimulationParameters.YukawaA, SimulationParameters.YukawaK, SimulationParameters.SystemSize);
switch (SimulationParameters.InitialConfiguration) {
case Random: {
pt.SetRotation(GetRandomUnitQuaternion());
pt.Coordinates = i > 0
? i * (SimulationParameters.ParticleDiameter / SimulationParameters.Density) +
initialDisplacementDistribution(rnd_gen)
: 0;
break;
}
case RandomUnmoving: {
pt.SetRotation(GetRandomUnitQuaternion());
pt.Coordinates = i * (SimulationParameters.ParticleDiameter / SimulationParameters.Density);
break;
}
case Aligned: {
pt.SetRotation(CQuaternion(0, CVector::AxisZ));
pt.Coordinates = i > 0
? i * (SimulationParameters.ParticleDiameter / SimulationParameters.Density) +
initialDisplacementDistribution(rnd_gen)
: 0;
break;
}
case AlignedTwoSides: {
pt.SetRotation(CQuaternion(M_PI * (i % 2), CVector::AxisY));
pt.Coordinates = i > 0
? i * (SimulationParameters.ParticleDiameter / SimulationParameters.Density) +
initialDisplacementDistribution(rnd_gen)
: 0;
break;
}
case AlingnedUnmoving: {
pt.SetRotation(CQuaternion(M_PI * (i % 2), CVector::AxisY));
pt.Coordinates = i * (SimulationParameters.ParticleDiameter / SimulationParameters.Density);
break;
}
case OneCluster: {
pt.SetRotation(CQuaternion(0, CVector::AxisZ));
pt.Coordinates = SimulationParameters.SystemSize / 4 + SimulationParameters.ParticleDiameter * i;
}
}
return pt;
}
CQuaternion CQuaternion::Slerp(CQuaternion &q1, CQuaternion &q2, float t)
{
CQuaternion qInterpolated;
if(q1.x == q2.x && q1.y == q2.y && q1.z == q2.z && q1.w == q2.w)
return q1;
float result = (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z) + (q1.w * q2.w);
if(result < 0.0f)
{
q2 = CQuaternion(-q2.x, -q2.y, -q2.z, -q2.w);
result = -result;
}
float scale0 = 1 - t, scale1 = t;
if(1 - result > 0.1f)
{
float theta = (float)acos(result);
float sinTheta = (float)sin(theta);
scale0 = (float)sin( ( 1 - t ) * theta) / sinTheta;
scale1 = (float)sin( ( t * theta) ) / sinTheta;
}
qInterpolated.x = (scale0 * q1.x) + (scale1 * q2.x);
qInterpolated.y = (scale0 * q1.y) + (scale1 * q2.y);
qInterpolated.z = (scale0 * q1.z) + (scale1 * q2.z);
qInterpolated.w = (scale0 * q1.w) + (scale1 * q2.w);
return qInterpolated;
}
CVector3 CQuaternion::operator* (const CVector3& v) const {
#if 0
CMatrix4 m;
toRotationMatrix(m);
return m * v;
#else
// nVidia SDK implementation
CVector3 uv, uuv;
CVector3 qvec(x, y, z);
uv = qvec ^ v;
uuv = qvec ^ uv;
uv *= (2.0f * w);
uuv *= 2.0f;
return v + uv + uuv;
#endif
#if 0
CQuaternion thiss = *this;
thiss.invert();
thiss = thiss * CQuaternion(v.x, v.y, v.z, 0.0);
thiss = thiss * *this;
return CVector3(thiss.x, thiss.y, thiss.z);
#endif
}
CLEDEntity::CLEDEntity(CComposableEntity* pc_parent,
const std::string& str_id,
const CVector3& c_position,
const CColor& c_color) :
CPositionalEntity(pc_parent, str_id, c_position, CQuaternion()),
m_cColor(c_color),
m_cInitColor(c_color) {}
void CPFA_qt_user_functions::DrawOnRobot(CFootBotEntity& entity) {
CPFA_controller& c = dynamic_cast<CPFA_controller&>(entity.GetControllableEntity().GetController());
if(c.IsHoldingFood() == true) {
DrawCylinder(CVector3(0.0, 0.0, 0.3), CQuaternion(), loopFunctions.FoodRadius, 0.025, CColor::BLACK);
}
if(loopFunctions.DrawIDs == 1) {
/* Disable lighting, so it does not interfere with the chosen text color */
glDisable(GL_LIGHTING);
/* Disable face culling to be sure the text is visible from anywhere */
glDisable(GL_CULL_FACE);
/* Set the text color */
CColor cColor(CColor::BLACK);
glColor3ub(cColor.GetRed(), cColor.GetGreen(), cColor.GetBlue());
/* The position of the text is expressed wrt the reference point of the footbot
* For a foot-bot, the reference point is the center of its base.
* See also the description in
* $ argos3 -q foot-bot
*/
// Disable for now
//GetOpenGLWidget().renderText(0.0, 0.0, 0.5, // position
// entity.GetId().c_str()); // text
/* Restore face culling */
glEnable(GL_CULL_FACE);
/* Restore lighting */
glEnable(GL_LIGHTING);
}
}
void CWorldEditor::CreateBaseAxis()
{
auto sceneRoot = m_overlayViewport->GetSceneRoot();
static const CVector3 g_arrowScale(0.075f, 0.25f, 0.075f);
{
auto baseAxisNode = Palleon::CSceneNode::Create();
baseAxisNode->SetPosition(CVector3(289.2f, 5.00f, -563.f));
sceneRoot->AppendChild(baseAxisNode);
{
auto axisMesh = Palleon::CAxisMesh::Create();
axisMesh->SetScale(CVector3(1, 1, 1));
baseAxisNode->AppendChild(axisMesh);
}
//X arrow
{
auto coneMesh = Palleon::CConeMesh::Create();
coneMesh->SetPosition(CVector3(1, 0, 0));
coneMesh->SetRotation(CQuaternion(CVector3(0, 0, 1), M_PI / 2.f));
coneMesh->SetScale(g_arrowScale);
coneMesh->GetMaterial()->SetColor(CColor(1, 0, 0, 1));
baseAxisNode->AppendChild(coneMesh);
}
//Y arrow
{
auto coneMesh = Palleon::CConeMesh::Create();
coneMesh->SetPosition(CVector3(0, 1, 0));
coneMesh->SetScale(g_arrowScale);
coneMesh->GetMaterial()->SetColor(CColor(0, 1, 0, 1));
baseAxisNode->AppendChild(coneMesh);
}
//Z arrow
{
auto coneMesh = Palleon::CConeMesh::Create();
coneMesh->SetPosition(CVector3(0, 0, 1));
coneMesh->SetRotation(CQuaternion(CVector3(1, 0, 0), -M_PI / 2.f));
coneMesh->SetScale(g_arrowScale);
coneMesh->GetMaterial()->SetColor(CColor(0, 0, 1, 1));
baseAxisNode->AppendChild(coneMesh);
}
}
}
CQuaternion CQuaternion::operator*(const CQuaternion& q)
{
// This is how to multiply a quaternion by a quaternion
return CQuaternion(w * q.x + x * q.w + y * q.z - z * q.y,
w * q.y + y * q.w + z * q.x - x * q.z,
w * q.z + z * q.w + x * q.y - y * q.x,
w * q.w - x * q.x - y * q.y - z * q.z);
}
void CFootBotTurretEntity::Reset() {
m_unMode = MODE_OFF;
m_cDesRot = CRadians::ZERO;
m_cOldRot = CRadians::ZERO;
m_fDesRotSpeed = 0.0;
m_fCurRotSpeed = 0.0;
m_psAnchor->OffsetOrientation = CQuaternion();
}
CQuaternion<T> operator+(const CQuaternion<T> &q)
{
CQuaternion quart = CQuaternion(w+q.w, i+q.i, j+q.j, k+q.k);
if (quart.getLength() != 1) quart.normalize();
return quart;
}
void CPFA_qt_user_functions::DrawPheromones() {
Real x, y, weight;
vector<CVector2> trail;
CColor trailColor = CColor::GREEN, pColor = CColor::GREEN;
for(size_t i = 0; i < loopFunctions.PheromoneList.size(); i++) {
x = loopFunctions.PheromoneList[i].GetLocation().GetX();
y = loopFunctions.PheromoneList[i].GetLocation().GetY();
if(loopFunctions.DrawTrails == 1) {
trail = loopFunctions.PheromoneList[i].GetTrail();
weight = loopFunctions.PheromoneList[i].GetWeight();
if(weight > 0.25 && weight <= 1.0) // [ 100.0% , 25.0% )
pColor = trailColor = CColor::GREEN;
else if(weight > 0.05 && weight <= 0.25) // [ 25.0% , 5.0% )
pColor = trailColor = CColor::YELLOW;
else // [ 5.0% , 0.0% ]
pColor = trailColor = CColor::RED;
CRay3 ray;
size_t j = 0;
for(j = 1; j < trail.size(); j++) {
ray = CRay3(CVector3(trail[j - 1].GetX(), trail[j - 1].GetY(), 0.01),
CVector3(trail[j].GetX(), trail[j].GetY(), 0.01));
DrawRay(ray, trailColor, 1.0);
}
DrawCylinder(CVector3(x, y, 0.0), CQuaternion(), loopFunctions.FoodRadius, 0.025, pColor);
} else {
weight = loopFunctions.PheromoneList[i].GetWeight();
if(weight > 0.25 && weight <= 1.0) // [ 100.0% , 25.0% )
pColor = CColor::GREEN;
else if(weight > 0.05 && weight <= 0.25) // [ 25.0% , 5.0% )
pColor = CColor::YELLOW;
else // [ 5.0% , 0.0% ]
pColor = CColor::RED;
DrawCylinder(CVector3(x, y, 0.0), CQuaternion(), loopFunctions.FoodRadius, 0.025, pColor);
}
}
}
// Return a normalised version of a quaternion (unit length as a 4-vector) - non-member version
CQuaternion Normalise
(
const CQuaternion& quat
)
{
TFloat32 fNormSquared = quat.w*quat.w + quat.x*quat.x + quat.y*quat.y + quat.z*quat.z;
if ( gen::IsZero( fNormSquared ) )
{
return CQuaternion( 0.0f, 0.0f, 0.0f, 0.0f );
}
else
{
TFloat32 fInvLength = InvSqrt( fNormSquared );
return CQuaternion( quat.w*fInvLength, quat.x*fInvLength,
quat.y*fInvLength, quat.z*fInvLength );
}
}
CQuaternion CQuaternion::operator* (const CQuaternion& q) const {
#if 0
return CQuaternion
(
w * q.x + x * q.w + y * q.z - z * q.y,
w * q.y + y * q.w + z * q.x - x * q.z,
w * q.z + z * q.w + x * q.y - y * q.x,
w * q.w - x * q.x - y * q.y - z * q.z
);
#else
return CQuaternion
(
w * q.x + x * q.w + y * q.z - z * q.y,
w * q.y - x * q.z + y * q.w + z * q.x,
w * q.z + x * q.y - y * q.x + z * q.w,
w * q.w - x * q.x - y * q.y - z * q.z
);
#endif
}
CVector3& CVector3::Rotate(const CQuaternion& c_quaternion) {
CQuaternion cResult;
cResult = c_quaternion;
cResult *= CQuaternion(0.0f, m_fX, m_fY, m_fZ);
cResult *= c_quaternion.Inverse();
m_fX = cResult.GetX();
m_fY = cResult.GetY();
m_fZ = cResult.GetZ();
return *this;
}
void CPFA_qt_user_functions::DrawFidelity() {
Real x, y;
for(size_t i = 0; i < loopFunctions.FidelityList.size(); i++) {
x = loopFunctions.FidelityList[i].GetX();
y = loopFunctions.FidelityList[i].GetY();
DrawCylinder(CVector3(x, y, 0.0), CQuaternion(), loopFunctions.FoodRadius, 0.025, CColor::CYAN);
}
}
// Return the quaternion result of multiplying two quaternions - non-member function
CQuaternion operator*
(
const CQuaternion& q1,
const CQuaternion& q2
)
{
const CVector3& v1 = q1.Vector();
const CVector3& v2 = q2.Vector();
return CQuaternion( q1.w*q2.w - Dot( v1, v2 ), q1.w*v2 + q2.w*v1 + Cross( v2, v1 ) );
}
CQuaternion Exp(CVector3LG const& w)
{
float theta = (float)sqrt(w % w);
float sc;
if(theta < EPSILON)
sc = 1.0f;
else
sc = (float)(sin(theta) / theta);
CVector3LG v = sc * w;
return CQuaternion((float)cos(theta), v.x(), v.y(), v.z());
}
CQuaternion CBaseSimCtrl::GetRandomUnitQuaternion() {
auto u1 = uniformDistributionZeroOne(rnd_gen);
auto u2 = uniformDistributionZeroTwoPi(rnd_gen);
auto u3 = uniformDistributionZeroTwoPi(rnd_gen);
auto ret = CQuaternion(
sqrt(1 - u1) * sin(u2),
sqrt(1 - u1) * cos(u2),
sqrt(u1) * sin(u3),
sqrt(u1) * cos(u3)
);
return ret;
}
void CDynamics3DEntity::UpdateEntityStatus() {
/* Update entity position and orientation */
const dReal* ptPosition = dBodyGetPosition(m_tBody);
GetEmbodiedEntity().SetPosition(
CVector3(ptPosition[0],
ptPosition[1],
ptPosition[2]));
const dReal* ptOrientation = dBodyGetQuaternion(m_tBody);
GetEmbodiedEntity().SetOrientation(
CQuaternion(ptOrientation[0],
ptOrientation[1],
ptOrientation[2],
ptOrientation[3]));
}
/*****
* This function is called by the DrawOnArena(...) function. If the iAnt_data
* object is not initialized this function should not be called.
*****/
void CPFA_qt_user_functions::DrawNest() {
/* 2d cartesian coordinates of the nest */
Real x_coordinate = loopFunctions.NestPosition.GetX();
Real y_coordinate = loopFunctions.NestPosition.GetX();
/* required: leaving this 0.0 will draw the nest inside of the floor */
Real elevation = loopFunctions.NestElevation;
/* 3d cartesian coordinates of the nest */
CVector3 nest_3d(x_coordinate, y_coordinate, elevation);
/* Draw the nest on the arena. */
DrawCircle(nest_3d, CQuaternion(), loopFunctions.NestRadius, CColor::GRAY50);
}
void CPositionalEntity::Init(TConfigurationNode& t_tree) {
try {
/* Initialize base entity */
CEntity::Init(t_tree);
/* Get the position of the entity */
GetNodeAttributeOrDefault(t_tree, "position", m_cPosition, CVector3());
/* Get the orientation of the entity */
GetNodeAttributeOrDefault(t_tree, "orientation", m_cOrientation, CQuaternion());
m_cInitPosition = m_cPosition;
m_cInitOrientation = m_cOrientation;
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize positional entity \"" << GetId() << "\".", ex);
}
}
// draw function for adding graphics to a foot-bot
void iAnt_qt_user_functions::DrawFood(CFootBotEntity& entity) {
iAnt_controller& c = dynamic_cast<iAnt_controller&>
(entity.GetControllableEntity().GetController());
UpdateDrawInWorldData(c);
if(c.IsHoldingFood() == true) {
//#ifdef __APPLE__
//Edit here for drawing trails
DrawCylinder(CVector3(0.0f, 0.0f, 0.3f), CQuaternion(), 0.05f, 0.025f, CColor::BLACK);
//#else
// DrawCylinder(0.05f, 0.025f, CVector3(0.0f, 0.0f, 0.3f), CColor::BLACK);
//#endif
}
}
CEmbodiedEntity::CEmbodiedEntity(CComposableEntity* pc_parent,
const std::string& str_id,
const CVector3& c_position,
const CQuaternion& c_orientation,
bool b_movable) :
CEntity(pc_parent, str_id),
m_bMovable(b_movable),
m_sBoundingBox(NULL),
m_psOriginAnchor(new SAnchor("origin",
0,
CVector3(),
CQuaternion(),
c_position,
c_orientation)),
m_cInitOriginPosition(c_position),
m_cInitOriginOrientation(c_orientation) {
/* Add anchor to map and enable it */
m_mapAnchors[m_psOriginAnchor->Id] = m_psOriginAnchor;
EnableAnchor("origin");
}
extern "C" int Function_ChangeBinaryToBaseParticles(void * input_string, void * output_string, int ptCount) {
std::string input((char*)input_string);
std::stringstream in_stream;
in_stream << "{\"value0\": \"";
in_stream << input;
in_stream << "\"}";
// std::cout << in_stream.str() << std::endl;
cereal::JSONInputArchive arch(in_stream);
std::vector<CWrongPtBase> particles;
for (int i = 0; i < ptCount; ++i) {
particles.push_back(CWrongPtBase());
}
arch.loadBinaryValue(&particles[0], sizeof(CWrongPtBase) * ptCount);
std::vector<CParticleBase> saveParticles;
for (int i = 0; i < ptCount; ++i){
auto pt = CParticleBase();
pt.Coordinates = particles[i].Coordinates;
CQuaternion newOrient;
newOrient.W = particles[i].Rotation.W;
newOrient.V = particles[i].Rotation.V;
pt.SetRotation(CQuaternion(newOrient));
saveParticles.push_back(pt);
}
std::stringstream out_stream;
cereal::JSONOutputArchive oarch(out_stream);
oarch.saveBinaryValue(&saveParticles[0], sizeof(CParticleBase)*ptCount);
std::string out_string = out_stream.str();
for(int i = 17; i < out_string.length()-1; i++) {
((char*)output_string)[i-17] = out_string[i];
}
return strlen((char*)output_string);
}
void applyAngularRotation(const CVector<3,T> &angular_rotation)
{
#if 0
// not working for unknown reasons
CQuaternion n = (*this)*CQuaternion(angular_rotation.data[0], angular_rotation.data[1], angular_rotation.data[2], 0);
i += n.i*0.5;
j += n.j*0.5;
k += n.k*0.5;
w += n.w*0.5;
normalize();
#else
float angle = angular_rotation.getLength();
if (angle <= 0)
{
return;
}
this->rotatePost(angular_rotation * (1.0f/angle), angle);
#endif
}
CQuaternion CMatrix3D::GetRotation() const
{
float tr = _data2d[0][0] + _data2d[1][1] + _data2d[2][2];
int next[] = { 1, 2, 0 };
float quat[4];
if (tr > 0.f)
{
float s = sqrtf(tr + 1.f);
quat[3] = s * 0.5f;
s = 0.5f / s;
quat[0] = (_data2d[1][2] - _data2d[2][1]) * s;
quat[1] = (_data2d[2][0] - _data2d[0][2]) * s;
quat[2] = (_data2d[0][1] - _data2d[1][0]) * s;
}
else
{
int i = 0;
if (_data2d[1][1] > _data2d[0][0]) i = 1;
if (_data2d[2][2] > _data2d[i][i]) i = 2;
int j = next[i];
int k = next[j];
float s = sqrtf((_data2d[i][i] - (_data2d[j][j] + _data2d[k][k])) + 1.f);
quat[i] = s * 0.5f;
if (s != 0.f) s = 0.5f / s;
quat[3] = (_data2d[j][k] - _data2d[k][j]) * s;
quat[j] = (_data2d[i][j] + _data2d[j][i]) * s;
quat[k] = (_data2d[i][k] + _data2d[k][i]) * s;
}
return CQuaternion(quat[0], quat[1], quat[2], quat[3]);
CQuaternion CQuaternion::SlerpTo(const CQuaternion& oQuat, double dAmount) const {
CLog("math","CQuaternion::SlerpTo");
double dOmega, dCos, dSin, dScale0, dScale1;
// Extract values from quaternions
double dThisX, dThisY, dThisZ, dThisW, dTgtX, dTgtY, dTgtZ, dTgtW;
m_oVector.ToDouble(dThisX,dThisY,dThisZ);
dThisW = m_dScalar;
oQuat.m_oVector.ToDouble(dTgtX,dTgtY,dTgtZ);
dTgtW = oQuat.m_dScalar;
// Calculate cosine
dCos = (dThisX * dTgtX) + (dThisY * dTgtY) + (dThisZ * dTgtZ) + (dThisW * dTgtW);
// Adjust signs if necessary
if (dCos < 0.0) {
dCos = -dCos;
dTgtX = -dTgtX;
dTgtY = -dTgtY;
dTgtZ = -dTgtZ;
dTgtW = -dTgtW;
}
// Calculate coeffecients
dOmega = acos(dCos);
dSin = sin(dOmega);
if (dSin != 0.0) {
// Slerping!
dScale0 = sin((1.0 - dAmount) * dOmega) / dSin;
dScale1 = sin(dAmount * dOmega) / dSin;
}
else {
// using linear to avoid divide-by-zero
dScale0 = 1.0 - dAmount;
dScale1 = dAmount;
}
// Calcalate result
double dResult = dScale0*dThisW+dScale1*dTgtW;
CVector3D oVecResult(dScale0*dThisX+dScale1*dTgtX,dScale0*dThisY+dScale1*dTgtY,dScale0*dThisZ+dScale1*dTgtZ);
return CQuaternion(dResult,oVecResult);
} //SlerpTo(const CQuaternion& oQuat, double dAmount) const
CQuaternion operator * ( CQuaternion& cQ1 ) {
return CQuaternion( cV.Cross(cQ1.cV) + cQ1.cV*fScalar + cV*cQ1.fScalar,
fScalar*cQ1.fScalar - cQ1.cV.Dot(cV) );
}
CQuaternion operator - ( CQuaternion& cQ1 ) const {
return CQuaternion( cV.fX - cQ1.cV.fX, cV.fY - cQ1.cV.fY, cV.fZ - cQ1.cV.fZ, fScalar - cQ1.fScalar );
}
CQuaternion operator + ( const CQuaternion& cQ1 ) const {
return CQuaternion( cV.fX + cQ1.cV.fX, cV.fY + cQ1.cV.fY, cV.fZ + cQ1.cV.fZ, fScalar + cQ1.fScalar );
}
