void PackageTarget::applyJoint (Package *package)
{
assert(_joint == nullptr);
const b2Vec2 shift(0.0f, -0.1f);
b2DistanceJointDef def;
def.Initialize(getBodies()[0], package->getBodies()[0], getBodies()[0]->GetWorldPoint(shift), package->getBodies()[0]->GetPosition());
_joint = static_cast<b2DistanceJoint*>(_map.getWorld()->CreateJoint(&def));
_lengthUpdate = LENGTH_UPDATE_DELAY;
package->setGravityScale(0.0f);
const bool bonus = package->setArrived();
if (!bonus)
return;
_map.addPoints(package->getLastDropper(), 30);
}
void WindModificator::update (uint32_t deltaTime)
{
IEntity::update(deltaTime);
b2Body *ownBody = getBodies()[0];
if (!_state) {
ownBody->SetActive(false);
return;
} else {
ownBody->SetActive(true);
}
for (b2ContactEdge* c = ownBody->GetContactList(); c != nullptr; c = c->next) {
b2Contact *contact = c->contact;
if (!contact->IsTouching())
continue;
b2Body *bodyA = contact->GetFixtureA()->GetBody();
b2Body *bodyB = contact->GetFixtureB()->GetBody();
b2Body *body = ownBody != bodyA ? bodyA : bodyB;
b2WorldManifold worldManifold;
contact->GetWorldManifold(&worldManifold);
const b2Manifold *manifold = contact->GetManifold();
for (int i = 0; i < manifold->pointCount; ++i) {
const b2Vec2& contactPoint = worldManifold.points[i];
applyImpulse(body, contactPoint, _force);
}
}
}
double System::getSystemEnergy() const {
double energy = 0.0;
for (identifier body : getBodies()) {
energy += getBodyKineticEnergy(body);
}
for (Interaction* interaction : interactions) {
energy += interaction->getEnergy(phase);
}
return energy;
}
std::pair<vector3D, vector3D> System::convertToCenterOfMassSystem() {
vector3D systemCenterOfMass = getSystemCenterOfMass();
vector3D systemVelocity = getSystemImpulse() / getSystemMass();
for (identifier body : getBodies()) {
setBodyPosition(body, getBodyPosition(body) - systemCenterOfMass);
setBodyVelocity(body, getBodyVelocity(body) - systemVelocity);
}
std::pair<vector3D, vector3D> center(systemCenterOfMass, systemVelocity);
return center;
}
//--------------------------------------------------------------
void ofApp::setup(){
kinect.open();
kinect.initDepthSource();
kinect.initColorSource();
kinect.initInfraredSource();
kinect.initBodyIndexSource();
kinect.initBodySource();
gui.init();
//setup a gui panel for the 3D view
auto worldView = gui.addWorld("World");
worldView->onDrawWorld += [this](ofCamera &) {
this->kinect.drawWorld();
};
//setup a gui panel for every kinect source
auto sources = kinect.getSources();
for(auto source : sources) {
auto sourceWithTexture = dynamic_pointer_cast<ofBaseHasTexture>(source);
if (sourceWithTexture) {
auto panel = gui.add(sourceWithTexture->getTexture(), source->getTypeName());
//if it's the colour panel, let's do something special by writing some info on top
auto colorSource = dynamic_pointer_cast<ofxKFW2::Source::Color>(source);
if (colorSource) {
panel->onDraw += [colorSource] (ofxCvGui::DrawArguments &) {
stringstream message;
message << "Exposure : " << colorSource->getExposure() << "us" << endl;
message << "FrameInterval : " << colorSource->getFrameInterval() << "us" << endl;
message << "Gain : " << colorSource->getGain() << endl;
message << "Gamma : " << colorSource->getGamma() << endl;
ofxCvGui::Utils::drawText(message.str(), 20, 60);
};
}
//if it's the depth panel, set some scaling
auto depthSource = dynamic_pointer_cast<ofxKFW2::Source::Depth>(source);
if (depthSource) {
auto style = make_shared<ofxCvGui::Panels::Texture::Style>();
style->rangeMaximum = 0.25f;
panel->setStyle(style);
}
//if it's the body index panel, let's draw the joints on top
auto bodyIndexSource = dynamic_pointer_cast<ofxKFW2::Source::BodyIndex>(source);
if(bodyIndexSource) {
panel->onDrawImage += [this](ofxCvGui::DrawImageArguments & args) {
auto bodySource = this->kinect.getBodySource();
const auto & bodies = bodySource->getBodies();
ofPushStyle();
{
ofColor color(200, 100, 100);
int index = 0;
for (const auto & body : bodies) {
color.setHueAngle((index * 50) % 360);
ofSetColor(color);
for (const auto & joint : body.joints) {
ofDrawCircle(joint.second.getPositionInDepthMap(), 5);
}
index++;
}
}
ofPopStyle();
};
}
}
}
//if we press the 'c' key on the World panel, then toggle the camera's cursor. This works best when you fullscreen that panel
worldView->onKeyboard += [this, worldView] (ofxCvGui::KeyboardArguments & args) {
if (args.action == ofxCvGui::KeyboardArguments::Action::Pressed && args.key =='c') {
worldView->getCamera().toggleCursorDrawEnabled();
}
};
}
vector3D System::getSystemImpulse() const {
return getGroupImpulse(getBodies());
}
vector3D System::getSystemCenterOfMass() const {
return getGroupCenterOfMass(getBodies());
}
double System::getSystemMass() const {
return getGroupMass(getBodies());
}
