void CDynamics3DMagnetismPlugin::RegisterModel(CDynamics3DModel& c_model) {
CComposableEntity& cEntity = c_model.GetComposableEntity();
if(cEntity.HasComponent("magnets")) {
/* Get the list of bodies belonging to this model */
std::vector<CDynamics3DModel::CAbstractBody*>& cModelBodies = c_model.GetBodies();
/* Get the list of magnets belonging to the entity */
CMagnetEquippedEntity::SInstance::TVector& vecInstances =
cEntity.GetComponent<CMagnetEquippedEntity>("magnets").GetInstances();
/* For each magnet */
for(CMagnetEquippedEntity::SInstance& s_instance : vecInstances) {
std::vector<CDynamics3DModel::CAbstractBody*>::iterator itMagneticBody =
std::find_if(std::begin(cModelBodies),
std::end(cModelBodies),
[&s_instance] (CDynamics3DModel::CAbstractBody* pc_body) {
/* If both the body and the magnet share the same anchor,
we have found our magnetic body */
return (&(s_instance.Anchor) == &(pc_body->GetAnchor()));
});
if(itMagneticBody != std::end(cModelBodies)) {
btTransform cOffset(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f),
btVector3(s_instance.Offset.GetX(),
s_instance.Offset.GetZ(),
-s_instance.Offset.GetY()));
cOffset *= (*itMagneticBody)->GetData().CenterOfMassOffset;
m_vecDipoles.emplace_back(**itMagneticBody, s_instance.Magnet, cOffset);
}
}
}
}
//--------------------------------------------------------------
void testApp::draw(){
ofBackground(0);
ofEnableAlphaBlending();
// make a little legend for the color
////////////////////////////////////////////////
int y = ofGetHeight() - 60;
int x = 10;
int h = 12;
int w = 30;
ofColor cSubject(255,0,0,200);
ofColor cMask(255,255,0,200);
ofColor cResult(0,255,0,200);
ofColor cOffset(0,0,255,200);
ofFill();
ofSetColor(cSubject);
ofRect(x,y-h+2,w,h);
ofSetColor(255);
ofDrawBitmapString("Polygon Clip Subjects", ofPoint(x + w + 2,y));
y+= 16;
ofFill();
ofSetColor(cMask);
ofRect(x,y-h+2,w,h);
ofSetColor(255);
ofDrawBitmapString("Polygon Clip Masks", ofPoint(x + w + 2,y));
y+= 16;
ofFill();
ofSetColor(cResult);
ofRect(x,y-h+2,w,h);
ofSetColor(255);
ofDrawBitmapString("Clip Results", ofPoint(x + w + 2,y));
y+= 16;
ofFill();
ofSetColor(cOffset);
ofRect(x,y-h+2,w,h);
ofSetColor(255);
ofDrawBitmapString("Offset Results", ofPoint(x + w + 2,y));
// draw all of the pieces
////////////////////////////////////////////////
ofPushMatrix();
ofTranslate(-ofGetWidth() / 4, 0);
clipSubjects.setStrokeColor(cSubject);
clipSubjects.setStrokeWidth(1.0);
clipSubjects.setFilled(false);
clipSubjects.draw();
clipMasks.setStrokeColor(cMask);
clipMasks.setStrokeWidth(1.0);
clipMasks.setFilled(false);
clipMasks.draw();
ofPopMatrix();
ofPushMatrix();
ofTranslate(ofGetWidth() / 4, 0);
clips.setStrokeColor(cResult);
clips.setStrokeWidth(1.0);
clips.setFilled(false);
clips.draw();
if(enableOffsetsToggle) {
offsets.setStrokeColor(cOffset);
offsets.setStrokeWidth(1.0);
offsets.setFilled(false);
offsets.draw();
}
ofPopMatrix();
ofPushMatrix();
ofTranslate(ofGetWidth() * 2 / 4, 100);
complexPath.draw();
for(int i = 0; i < simplifiedPath.getOutline().size(); i++) {
ofSetColor(255,255,0);
simplifiedPath.getOutline()[i].draw();
}
ofPopMatrix();
// draw the gui
////////////////////////////////////////////////
clipTypePanel.draw();
offsetPanel.draw();
ofDisableAlphaBlending();
}
void CCameraDefaultSensor::Init(TConfigurationNode& t_tree) {
try {
/* Parent class init */
CCI_CameraSensor::Init(t_tree);
/* Show the frustums */
GetNodeAttributeOrDefault(t_tree, "show_frustum", m_bShowFrustum, m_bShowFrustum);
/* For each camera */
TConfigurationNodeIterator itCamera("camera");
for(itCamera = itCamera.begin(&t_tree);
itCamera != itCamera.end();
++itCamera) {
/* Get camera indentifier */
std::string strId;
GetNodeAttribute(*itCamera, "id", strId);
/* Parse and look up the anchor */
std::string strAnchorId;
GetNodeAttribute(*itCamera, "anchor", strAnchorId);
SAnchor& sAnchor = m_pcEmbodiedEntity->GetAnchor(strAnchorId);
/* parse the offset */
CVector3 cOffsetPosition;
CQuaternion cOffsetOrientation;
GetNodeAttribute(*itCamera, "position", cOffsetPosition);
GetNodeAttribute(*itCamera, "orientation", cOffsetOrientation);
CTransformationMatrix3 cOffset(cOffsetOrientation, cOffsetPosition);
/* parse the range */
CRange<Real> cRange;
GetNodeAttribute(*itCamera, "range", cRange);
/* create the projection matrix */
CSquareMatrix<3> cProjectionMatrix;
cProjectionMatrix.SetIdentityMatrix();
/* set the focal length */
CVector2 cFocalLength;
GetNodeAttribute(*itCamera, "focal_length", cFocalLength);
cProjectionMatrix(0,0) = cFocalLength.GetX(); // Fx
cProjectionMatrix(1,1) = cFocalLength.GetY(); // Fy
/* set the principal point */
CVector2 cPrincipalPoint;
GetNodeAttribute(*itCamera, "principal_point", cPrincipalPoint);
cProjectionMatrix(0,2) = cPrincipalPoint.GetX(); // Px
cProjectionMatrix(1,2) = cPrincipalPoint.GetY(); // Py
/* set the distortion parameters */
/*
CMatrix<1,5> cDistortionParameters;
std::string strDistortionParameters;
Real pfDistortionParameters[3];
GetNodeAttribute(*itCamera, "distortion_parameters", strDistortionParameters);
ParseValues<Real>(strDistortionParameters, 3, pfDistortionParameters, ',');
cDistortionParameters(0,0) = pfDistortionParameters[0]; // K1
cDistortionParameters(0,1) = pfDistortionParameters[1]; // K2
cDistortionParameters(0,4) = pfDistortionParameters[2]; // K3
*/
/* parse the resolution */
CVector2 cResolution;
GetNodeAttribute(*itCamera, "resolution", cResolution);
/* create and initialise the algorithms */
std::vector<CCameraSensorSimulatedAlgorithm*> vecSimulatedAlgorithms;
std::vector<CCI_CameraSensorAlgorithm*> vecAlgorithms;
TConfigurationNodeIterator itAlgorithm;
for(itAlgorithm = itAlgorithm.begin(&(*itCamera));
itAlgorithm != itAlgorithm.end();
++itAlgorithm) {
/* create the algorithm */
CCameraSensorSimulatedAlgorithm* pcAlgorithm =
CFactory<CCameraSensorSimulatedAlgorithm>::New(itAlgorithm->Value());
/* check that algorithm inherits from a control interface */
CCI_CameraSensorAlgorithm* pcCIAlgorithm =
dynamic_cast<CCI_CameraSensorAlgorithm*>(pcAlgorithm);
if(pcCIAlgorithm == nullptr) {
THROW_ARGOSEXCEPTION("Algorithm \"" << itAlgorithm->Value() <<
"\" does not inherit from CCI_CameraSensorAlgorithm");
}
/* initialize the algorithm's control interface */
pcCIAlgorithm->Init(*itAlgorithm);
/* store pointers to the algorithms */
vecSimulatedAlgorithms.push_back(pcAlgorithm);
vecAlgorithms.push_back(pcCIAlgorithm);
}
/* create the simulated sensor */
m_vecSensors.emplace_back(sAnchor, cOffset, cRange, cProjectionMatrix,
cResolution, vecSimulatedAlgorithms);
/* create the sensor's control interface */
m_vecInterfaces.emplace_back(strId, vecAlgorithms);
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing camera sensor", ex);
}
Update();
}
