void PathMoveSystem::update(EntityPtr entity)
{
// @todo: some of this logic can be passed to component functions!
if(!HasCmpt(PathFindComponent, entity))
return;
GetCmpt(PathFindComponent, path_com, entity);
GetCmpt(PositionComponent, pos_com, entity);
GetCmpt(MovementComponent, mov_com, entity);
if(path_com->path.empty())
return;
EntityPtr next_waypoint = path_com->path.front();
GetCmpt(PositionComponent, wp_pos_com, next_waypoint);
Vec2f between = wp_pos_com->position - pos_com->position;
float dist = between.length();
if(dist <= 0.01f) // remove target
{
mov_com->speed = Vec2f(0.0f, 0.0f);
next_waypoint->mark_deleted();
path_com->path.pop_front();
}
else //Keep moving
{
between.length(0.1f);
mov_com->speed = between;
}
}
Vec2f BaseParticleSpring::normalize()
{
if( p1->fixed && p2->fixed )
return Vec2f( 0, 0 );
BaseParticle *particleBase = p1;
BaseParticle *particleNorm = p2;
if( p2->fixed )
{
particleBase = p2;
particleNorm = p1;
}
Vec2f direction = particleNorm->position - particleBase->position;
float d = direction.length();
if( d != distance )
{
direction.normalize();
Vec2f posNorm = particleNorm->position;
particleNorm->position = particleBase->position + direction * distance;
direction = particleNorm->position - particleBase->position;
d = direction.length();
return Vec2f( posNorm - particleNorm->position );
}
return Vec2f( 0, 0 );
}
void collide(Tile* tile)
{
Vec2f vec = tile->pos - pos;
float rads = math<float>::atan2(vec.x, vec.y) + M_PI;
if(vec.length() < 2 * TILERAD_MIN + 5.0f && vec.length() > 2 * TILERAD_MIN - 5.0f)
{
//
for(int i = 0; i < 6; i++)
{
if(rads < i * M_PI/3.0f + M_PI/24.0f && rads > i * M_PI/3.0f - M_PI/24.0f)
{
//console() << toDegrees(rads) << endl;
connect(tile, i);
tile->connect(this, (3 + i) % 6);
}
}
}
else
{
disconnect(tile);
tile->disconnect(this);
}
}
// Translate branch on 0.1 of vector [v1,v2]
void RefinementState::stretchBranch (const Filter &branch, const RefinementState &state, int v1_idx, int v2_idx, int val)
{
int i;
float r, sh = 0.1f * val;
const Vec2f &v1 = state.layout[v1_idx];
const Vec2f &v2 = state.layout[v2_idx];
Vec2f d;
d.diff(v2, v1);
r = d.length();
if (r < EPSILON)
throw Error("too small edge");
d.scale(sh / r);
if (branch.valid(v1_idx))
d.negate();
layout.clear_resize(state.layout.size());
for (i = _graph.vertexBegin(); i < _graph.vertexEnd(); i = _graph.vertexNext(i))
{
if (!branch.valid(i))
layout[i].sum(state.layout[i], d);
else
layout[i] = state.layout[i];
}
}
void ParticleEmitter::repulseParticles()
{
for( list<Particle>::iterator p1 = mParticles.begin(); p1 != mParticles.end(); ++p1 ){
list<Particle>::iterator p2 = p1;
for( ++p2; p2 != mParticles.end(); ++p2 ) {
Vec2f dir = p1->mLoc - p2->mLoc;
float thresh = ( p1->mRadius + p2->mRadius ) * 5.0f;
if( dir.x > -thresh && dir.x < thresh && dir.y > -thresh && dir.y < thresh ){
float distSqrd = dir.lengthSquared() * dir.length();
if( distSqrd > 0.0f ){
float F = 1.0f/distSqrd;
dir.normalize();
// acceleration = force / mass
p1->mAcc += ( F * dir ) / p1->mMass;
p2->mAcc -= ( F * dir ) / p2->mMass;
// TMP
p1->mAcc *= 0.005;
p2->mAcc *= 0.005;
}
}
}
}
}
// Alignment
// For every nearby boid in the system, calculate the average velocity
Vec2f Boid::align(vector<Boid> &boids) {
float neighbordist = 15.0;
Vec2f steer;
int count = 0;
for (int i = 0 ; i < boids.size(); i++) {
Boid &other = boids[i];
float d = loc.distance(other.loc);
if ((d > 0) && (d < neighbordist)) {
steer += (other.vel);
count++;
}
}
if (count > 0) {
steer /= (float)count;
}
// As long as the vector is greater than 0
float mag = steer.length();
if (mag > 0) {
// Implement Reynolds: Steering = Desired - Velocity
steer /= mag;
steer *= maxspeed;
steer -= vel;
steer.x = clamp(steer.x, -maxforce, maxforce);
steer.y = clamp(steer.y, -maxforce, maxforce);
}
return steer;
}
void Star::drawName( const Vec2f &mousePos, float power, float alpha )
{
if( mDistToCam > 0.0f && mNameTex ){
float per = constrain( 1.0f - mDistToCam * 0.0000375f, 0.0f, 1.0f );
per *= per * per;
Vec2f dirToMouse = mScreenPos - mousePos;
mDistToMouse = dirToMouse.length();
if( mDistToMouse < 40.0f )
per += 1.0f - mDistToMouse/40.0f;
if( mIsSelected )
per = 1.0f;
if( per > 0.05f ){
gl::color( ColorA( power, power, power, per * alpha ) );
mNameTex.enableAndBind();
gl::draw( mNameTex, mScreenPos + Vec2f( mScreenRadius + 35.0f, -28.0f ) );
mNameTex.disable();
gl::color( ColorA( power, power, power, per * 0.4f * alpha ) );
Vec2f p1 = mScreenPos;
Vec2f p2 = p1 + Vec2f( mScreenRadius + 35.0f, -13.0f );
Vec2f p3 = mScreenPos + Vec2f( mScreenRadius + 35.0f + mNameTex.getWidth(), -13.0f );
gl::drawLine( p1, p2 );
gl::drawLine( p2, p3 );
}
}
}
/**
* Scales the input vector to the new length and returns it.
*/
Vec2f PathFitter::v2Scale(Vec2f v, float newlen) {
float len = v.length();
if (len != 0.0) {
v.x *= newlen/len;
v.y *= newlen/len;
}
return v;
}
Vec2f Food::attract(SnakeRef snake) {
Vec2f force = mLocation - snake->getLocation();
float distance = force.length();
distance = math<float>::clamp(distance, 10.0f, 25.0f);
float strength = (mAmount * snake->getHunger()) / ( distance * distance ) / 10;
force.safeNormalize();
force *= strength;
return force;
}
Vec2f Repeller::repel(Particle * p) {
Vec2f dir = location - p->location;
float d = constrain((float)dir.length(), 5.0f, 100.0f);
dir.safeNormalize();
float G = 1200;
float force = -1.0f * G / ( d * d );
return dir * force;
}
OSG_BASE_DLLMAPPING
void extend(CylinderVolume &srcVol, const CylinderVolume &vol)
{
Pnt3f min, max, min1, max1, min2, max2, apos;
Vec2f p;
Vec3f adir;
Real32 r;
if((!srcVol.isValid () && !srcVol.isEmpty()) ||
srcVol.isInfinite() ||
srcVol.isStatic () )
{
return;
}
if(!vol.isValid())
return;
if(srcVol.isEmpty())
{
if(vol.isEmpty())
{
return;
}
else
{
srcVol = vol;
return;
}
}
else if(vol.isEmpty())
{
return;
}
srcVol.getBounds(min, max);
vol .getBounds(min1, max1);
min2 = Pnt3f(osgMin(min.x(), min1.x()),
osgMin(min.y(), min1.y()),
osgMin(min.z(), min1.z()));
max2 = Pnt3f(osgMax(max.x(), max1.x()),
osgMax(max.y(), max1.y()),
osgMax(max.z(), max1.z()));
p = Vec2f(max2.x() - min2.x(), max2.y() - min2.y());
r = (p.length()) * 0.5f;
adir = Vec3f(0.f, 0.f, max2.z() - min2.z());
apos = Pnt3f(p.x(), p.y(), min2.z());
srcVol.setValue(apos, adir, r);
return;
}
void Particle::pullToCenter(const Vec2f ¢er)
{
Vec2f dirToCenter = m_loc - center;
float distToCenter = dirToCenter.length();
if (distToCenter > voidnoise::Settings::get().gravityDistance)
{
m_acc -= dirToCenter.normalized() *
((distToCenter - voidnoise::Settings::get().gravityDistance) * voidnoise::Settings::get().gravity);
}
}
Node* CanvasComponent::getNodeBelow(const cease::MouseEvent& event)
{
for (int i=0; i<inputNodes.size(); i++)
{
Vec2f distance = event.getPos() - inputNodes[i]->getCanvasPos();
if (distance.length() < 5) {
return inputNodes[i];
}
}
for (int i=0; i<outputNodes.size(); i++)
{
Vec2f distance = event.getPos() - outputNodes[i]->getCanvasPos();
if (distance.length() < 5) {
return outputNodes[i];
}
}
return NULL;
}
void Spring::connect( Mover * mover )
{
Vec2f force = mover->position - anchor;
float d = force.length();
float stretch = d - len;
force.safeNormalize();
force *= -1 * k * stretch;
mover->applyForce(force);
}
Vec2f Mover::attract(MoverRef m) {
float G = 0.4;
Vec2f force = mLocation - m->mLocation;
float distance = force.length();
distance = math<float>::clamp(distance, 5.0, 25.0);
force.safeNormalize();
float strength = (G * mMass * m->mMass) / (distance * distance);
force *= strength;
return force;
}
void FluidSDF::reconstructSurface(Particles::Ptr particles, float R, float r)
{
LOG_OUTPUT("Reconstructing fluid surface.");
assert(R && r);
_sum.reset();
_pAvg.reset();
int nx = _phi.nx();
int ny = _phi.ny();
for (int p = 0; p < particles->numParticles(); ++p) {
const int ci = particles->pos(p).x / _phi.dx();
const int cj = particles->pos(p).y / _phi.dx();
assert(ci < _phi.nx() && cj < _phi.ny());
for (int i = std::max(0,ci-2); i < std::min(nx,ci+2); ++i) {
for (int j = std::max(0,cj-2); j < std::min(ny,cj+2); ++j) {
const Vec2f xd = particles->pos(p) - _phi.pos(i,j);
const float k = _kernel(xd.length() / R);
_sum(i,j) += k;
_pAvg(i,j) += k * particles->pos(p);
}
}
}
for (int i = 0; i < _phi.nx(); ++i) {
for (int j = 0; j < _phi.ny(); ++j) {
if (_sum(i,j) > 0) {
_pAvg(i,j) *= (1.0f / _sum(i,j));
const Vec2f xd = _phi.pos(i,j) - _pAvg(i,j);
_phi(i,j) = xd.length() - r;
} else {
_phi(i,j) = _phi.dx() * 1e15;
}
}
}
}
Connector* Level::pickConnector(Vec2f atPosition, vector<Connector*> &connectors)
{
if (connectors.size() > 0) {
vector<Connector*>::iterator iter;
for (iter = connectors.begin(); iter != connectors.end(); iter++) {
Connector* connector = (*iter);
Vec2f distance = connector->getPosition() - Vec2f(atPosition.x, atPosition.y);
float radius = 25;
if (distance.length() < radius) {
return connector;
}
}
}
return 0;
}
void GLViewController::roll_ball(const CGLA::Vec2i& pos)
{
static Vec2i old_pos = pos;
Vec2f dir = Vec2f(pos-old_pos);
float len = dir.length();
if (len < TINY)
return;
ball.roll_ball(pos);
if(last_action==ZOOM_ACTION)
set_near_and_far();
spin = len>=1.1f;
old_pos = pos;
}
OSG_BASE_DLLMAPPING
void extend(CylinderVolume &srcVol, const Volume &vol)
{
const Volume *v = &vol;
const CylinderVolume *cylinder;
#ifndef OSG_2_PREP
const DynamicVolume *dynamic = dynamic_cast<const DynamicVolume *>(v);
if(dynamic)
{
v = &(dynamic->getInstance());
}
#endif
if((cylinder = dynamic_cast<const CylinderVolume *>(v)) != NULL)
{
OSG::extend(srcVol, *cylinder);
}
else
{
CylinderVolume localCylinder;
Pnt3f min, max, apos;
Vec3f adir;
Real32 r;
Vec2f p;
v->getBounds(min, max);
p = Vec2f(max.x() - min.x(), max.y() - min.y());
r = (p.length()) * 0.5f;
adir = Vec3f(0.f, 0.f, max.z() - min.z());
apos = Pnt3f(p.x(), p.y(), min.z());
localCylinder.setValue(apos, adir, r);
OSG::extend(srcVol, localCylinder);
}
return;
}
// Renders a vector object 'v' as an arrow and a location 'loc'
void NOC_4_08_ParticleSystemSmokeApp::drawVector( Vec2f v, Vec2f loc, float scale )
{
glPushMatrix();
float arrowsize = 4.0;
// Translate to location to render vector
gl::translate( loc );
gl::color( Color::white() );
// There is no heading2d function in cinder so get direction this way (note that pointing up is a heading of 0) and rotate
gl::rotate( toDegrees( atan2( v.y, v.x ) ) );
// Calculate length of vector & scale it to be bigger or smaller if necessary
float len = v.length() * scale;
// Draw three lines to make an arrow (draw pointing up since we've rotate to the proper direction)
gl::drawLine( Vec2f::zero(), Vec2f( len , 0.0 ) );
gl::drawLine( Vec2f( len, 0 ), Vec2f( len - arrowsize , arrowsize / 2 ) );
gl::drawLine( Vec2f( len, 0 ), Vec2f( len - arrowsize , -arrowsize / 2 ) );
glPopMatrix();
}
// Separation
// Method checks for nearby boids and steers away
Vec2f Boid::separate(vector<Boid> &boids) {
float desiredseparation = 10.0f;
Vec2f steer;
int count = 0;
// For every boid in the system, check if it's too close
for (int i = 0 ; i < boids.size(); i++) {
Boid &other = boids[i];
float d = loc.distance(other.loc);
// If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself)
if ((d > 0) && (d < desiredseparation)) {
// Calculate vector pointing away from neighbor
Vec2f diff = loc - other.loc;
diff /= d; // normalize
diff /= d; // Weight by distance
steer += diff;
count++; // Keep track of how many
}
}
// Average -- divide by how many
if (count > 0) {
steer /= (float)count;
}
// As long as the vector is greater than 0
//float mag = sqrt(steer.x*steer.x + steer.y*steer.y);
float mag = steer.length();
if (mag > 0) {
// Implement Reynolds: Steering = Desired - Velocity
steer /= mag;
steer *= maxspeed;
steer -= vel;
steer.x = clamp(steer.x, -maxforce, maxforce);
steer.y = clamp(steer.y, -maxforce, maxforce);
}
return steer;
}
// Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
// if we are away from the center of the sphere.
float QuatTrackBall::projectToSphere(const Vec2f& v)
{
#ifndef M_SQRT_2
const double M_SQRT_2 = 0.707106781187;
#endif
float d = v.length();
float t = ballsize*M_SQRT_2;
float z;
// Inside sphere
if(d < ballsize)
z = sqrt(ballsize*ballsize - d*d);
else if(d < t)
z = 0.0;
// On hyperbola
else
z = t*t/d;
return z;
}
void TrafficSimulation::update() {
networkDataMutex.lock();
// Update the position of the player to be send to the server
if (player != NULL) {
// Move the vehicle
Value vehicle;
vehicle["id"] = 0;
Value pos;
Vec3f worldPosition = player->getFrom();
pos[0u] = worldPosition[0];
pos[1] = worldPosition[1];
pos[2] = worldPosition[2];
vehicle["pos"] = pos;
// Pack as the requested array entry
dataToSend["moveVehicles"][0] = vehicle;
}
if (!receivedData.isNull()) {
//cout << "Received data\n";
// New data received per network. Update the positions of vehicles and the states of traffic lights
// A set which contains the IDs of the currently existing vehicles.
// If data for one of those is received, the entry in the set will be removed.
// All vehicles that are in the set after the loop finished are no longer in the
// area and can be deleted.
set<uint> vehicleIDs;
for (auto iter : vehicles) vehicleIDs.insert(iter.first);
if (!receivedData["vehicles"].isNull() && receivedData["vehicles"].isArray()) {
// Update the vehicles
//cout << "Received vehicles " << receivedData["vehicles"].size();
// A set for possible collisions
// Add them all to the set and resolve them later on to avoid doubled checks
set< pair<unsigned int, unsigned int> > collisions;
// Sleeps for 10ms each tick, but the send deltas are for one second
// Use only a part of them to avoid moving too fast
static const float partDelta = 10 / 1000;
for (auto vehicleIter : receivedData["vehicles"]) {
// Check if the values have valid types
if (!vehicleIter["id"].isConvertibleTo(uintValue)
|| !vehicleIter["pos"].isConvertibleTo(arrayValue)
|| !vehicleIter["pos"][0].isConvertibleTo(realValue)
|| !vehicleIter["pos"][1].isConvertibleTo(realValue)
|| !vehicleIter["pos"][2].isConvertibleTo(realValue)
|| !vehicleIter["dPos"].isConvertibleTo(arrayValue)
|| !vehicleIter["dPos"][0].isConvertibleTo(realValue)
|| !vehicleIter["dPos"][1].isConvertibleTo(realValue)
|| !vehicleIter["dPos"][2].isConvertibleTo(realValue)
|| !vehicleIter["angle"].isConvertibleTo(arrayValue)
|| !vehicleIter["angle"][0].isConvertibleTo(realValue)
|| !vehicleIter["angle"][1].isConvertibleTo(realValue)
|| !vehicleIter["angle"][2].isConvertibleTo(realValue)
|| !vehicleIter["dAngle"].isConvertibleTo(arrayValue)
|| !vehicleIter["dAngle"][0].isConvertibleTo(realValue)
|| !vehicleIter["dAngle"][1].isConvertibleTo(realValue)
|| !vehicleIter["dAngle"][2].isConvertibleTo(realValue)) {
cout << "TrafficSimulation: Warning: Received invalid vehicle data.\n";
continue;
}
uint ID = vehicleIter["id"].asUInt();
if (vehicles.count(ID) == 0) { // The vehicle is new, create it
if (!vehicleIter["vehicle"].isConvertibleTo(uintValue)
|| !vehicleIter["driver"].isConvertibleTo(uintValue)) {
cout << "TrafficSimulation: Warning: Received invalid vehicle data.\n";
continue;
}
uint vID = vehicleIter["vehicle"].asUInt();
if (meshes.count(vID) == 0) {
// If it is bigger than 500 it is our user-controlled vehicle
if (vID < 500) cout << "TrafficSimulation: Warning: Received unknown vehicle type " << vID << ".\n";
continue;
}
Vehicle v;
v.id = ID;
v.vehicleTypeId = vID;
v.driverTypeId = vehicleIter["driver"].asUInt();
if (meshes.count(v.vehicleTypeId) == 0) v.vehicleTypeId = 404;
v.geometry = (VRGeometry*)meshes[v.vehicleTypeId]->duplicate(true);
v.geometry->addAttachment("dynamicaly_generated", 0);
// Add it to the map
vehicles.insert(make_pair(v.id, v));
} else vehicleIDs.erase(ID); // Already (and still) exists, remove its ID from the vehicle-id-set
// Now the vehicle exists, update its position and state
Vehicle& v = vehicles[ID];
v.pos = Vec3f(vehicleIter["pos"][0].asFloat(), vehicleIter["pos"][1].asFloat(), vehicleIter["pos"][2].asFloat());
v.deltaPos = Vec3f(vehicleIter["dPos"][0].asFloat(), vehicleIter["dPos"][1].asFloat(), vehicleIter["dPos"][2].asFloat());
v.deltaPos *= partDelta;
v.orientation = Vec3f(vehicleIter["angle"][0].asFloat(), vehicleIter["angle"][1].asFloat(), vehicleIter["angle"][2].asFloat());
v.deltaOrientation = Vec3f(vehicleIter["dAngle"][0].asFloat(), vehicleIter["dAngle"][1].asFloat(), vehicleIter["dAngle"][2].asFloat());
v.deltaOrientation *= partDelta;
if (!vehicleIter["state"].isNull() && vehicleIter["state"].isArray()) {
v.state = Vehicle::NONE;
for (auto state : vehicleIter["state"]) {
if (!state.isConvertibleTo(stringValue)) continue;
if(state.asString() == "rightIndicator") {
v.state |= Vehicle::RIGHT_INDICATOR;
} else if(state.asString() == "leftIndicator") {
v.state |= Vehicle::LEFT_INDICATOR;
} else if(state.asString() == "accelerating") {
v.state |= Vehicle::ACCELERATING;
} else if(state.asString() == "braking") {
v.state |= Vehicle::BRAKING;
} else if(state.asString() == "waiting") {
v.state |= Vehicle::WAITING;
} else if(state.asString() == "blocked") {
v.state |= Vehicle::BLOCKED;
} else if(state.asString() == "collision") {
v.state |= Vehicle::COLLIDED;
}
}
}
if (!vehicleIter["colliding"].isNull() && vehicleIter["colliding"].isArray()) {
for (auto collisionIter : vehicleIter["colliding"]) {
if (!collisionIter.isConvertibleTo(uintValue)) continue;
uint other = collisionIter.asUInt();
if (other < v.id) collisions.insert(make_pair(other, v.id));
else collisions.insert(make_pair(v.id, other));
}
}
} // End vehicle iteration
// Okay, all vehicles are updated now
// Resolve collisions
if (collisionHandler != NULL) {
for (auto c : collisions) {
if (collisionHandler(vehicles[c.first], vehicles[c.second])) {
dataToSend["collision"].append(c.first);
dataToSend["collision"].append(c.second);
}
}
}
} // End vehicle updates
// Remove vehicles which are no longer on the map
for (auto v : vehicleIDs) {
vehicles[v].geometry->destroy();
vehicles.erase(v);
}
// Get traffic light updates
if (!receivedData["trafficlights"].isNull() && receivedData["trafficlights"].isArray()) {
// The light bulbs in the array will be moved around arbitrary whichever light posts are given
// If there are not enough bulbs in the array, more are added
// If there are too many bulbs, they are deleted
size_t bulbIndex = 0;
static const double postHeight = 2;
static const double bulbSize = 1; // Note: If you change this value from 2, change the value further down in new VRGeometry(), too.
for (auto lightpost : receivedData["trafficlights"]) {
if (!lightpost.isObject()) continue;
if (!lightpost["at"].isConvertibleTo(uintValue)
|| !lightpost["to"].isConvertibleTo(uintValue)
|| lightpost["state"].isNull()
|| !lightpost["state"].isArray()) {
cout << "TrafficSimulation: Warning: Received invalid light post data.\n";
continue;
}
// Calculate the vector of the street
// Get the node positions
Vec2f atPos, toPos;
string atId = lexical_cast<string>(lightpost["at"].asUInt());
string toId = lexical_cast<string>(lightpost["to"].asUInt());
bool foundAt = false, foundTo = false;
for (auto mapIter : loadedMaps) {
for (auto nodeIter : mapIter->osmNodes) {
if (!foundAt && nodeIter->id == atId) {
atPos = mapCoordinator->realToWorld(Vec2f(nodeIter->lat, nodeIter->lon));
foundAt = true;
}
if (!foundTo && nodeIter->id == toId) {
toPos = mapCoordinator->realToWorld(Vec2f(nodeIter->lat, nodeIter->lon));
foundTo = true;
}
if (foundAt && foundTo)
break;
}
if (foundAt && foundTo)
break;
}
Vec2f streetOffset = toPos - atPos;
const float prevLength = streetOffset.length();
streetOffset.normalize();
streetOffset *= min(prevLength / 2, Config::get()->STREET_WIDTH);
Vec2f normal(-streetOffset[1], streetOffset[0]);
normal.normalize();
normal *= Config::get()->STREET_WIDTH;
streetOffset += atPos;
// streetOffset now contains a position in the center of a street a bit away from the crossing
// normal is a vector that is orthogonal to the street
// Now iterate over the lanes and hang up the lights
double lane = -0.5;
for (auto light : lightpost["state"]) {
lane += 1;
if (!light.isConvertibleTo(stringValue)) continue;
while (bulbIndex+1 >= lightBulbs.size()) {
if (VRSceneManager::get()->getActiveScene() == NULL) break;
// Create a new light
VRGeometry* geo = new VRGeometry("ampel");
geo->addAttachment("dynamicaly_generated", 0);
geo->setPrimitive("Sphere", "0.5 2"); // The first value has to be half of bulbSize
geo->setMaterial(a_red);
VRSceneManager::get()->getActiveScene()->add(geo);
lightBulbs.push_back(geo);
}
// color switch
VRGeometry* bulb = lightBulbs[bulbIndex++];
Vec3f p = Vec3f(streetOffset[0] + lane * normal[0], postHeight, streetOffset[1] + lane * normal[1]);
string lcol = light.asString();
if (lcol == "red") {
bulb->setWorldPosition(p+Vec3f(0,3 * bulbSize,0));
bulb->setMaterial(a_red);
} else if (lcol == "redamber") {
bulb->setWorldPosition(p+Vec3f(0,3 * bulbSize,0));
bulb->setMaterial(a_red);
bulb = lightBulbs[bulbIndex++];
bulb->setWorldPosition(p+Vec3f(0,2 * bulbSize,0));
bulb->setMaterial(a_orange);
} else if (lcol == "amber") {
bulb->setWorldPosition(p+Vec3f(0,2 * bulbSize,0));
bulb->setMaterial(a_orange);
} else if (lcol == "green") {
bulb->setWorldPosition(p+Vec3f(0,bulbSize,0));
bulb->setMaterial(a_green);
}
}
}
// Remove unused lightbulbs
while (bulbIndex < lightBulbs.size()) {
lightBulbs.back()->destroy();
lightBulbs.pop_back();
}
}
}
networkDataMutex.unlock();
// Advance the vehicles a bit
//cout << "Update " << vehicles.size() << " vehicles\n";
for (auto v : vehicles) {
Vec3f p = v.second.pos;
p[1] = -1.2;//TODO: get right street height
v.second.geometry->setFrom(p);
v.second.geometry->setDir(v.second.pos - v.second.orientation);
v.second.pos += v.second.deltaPos;
v.second.orientation += v.second.deltaOrientation;
}
}
int Flocking::update()
{
int i;
Vec2f centroid(0,0);
for(i = 0; i < boids.size(); i++)
{
centroid+=boids[i].loc;
if(boids[i].isHit(destination.x,destination.y,destinationArea))
{
boids[i].reachedDestination = true;
}
if(useCollisionFromSDF)
{
Vec2f dir = partialDerivaties[(int)boids[i].loc.x][(int)boids[i].loc.y];
float val = collisionSDF[(int)boids[i].loc.x][(int)boids[i].loc.y];
if(val==0)
val=0.001;
boids[i].seek(dir+boids[i].loc,dir.length()*collisionWeight/val);
}
if(sceneMap->getCell(boids[i].loc.x,boids[i].loc.y))
{
/*
vector<Vec2f> path;
path = pathFinder.getPath(sceneMap,boids[i].loc);
if(path.size()>1)
{
Vec2f dest = path[min((int)path.size()-1,10)]; //!@#
boids[i].seek(dest,destWeight); //seek the Goal !@#
}
*/
}
else
{
boids[i].hitObstacle = true;
}
//boids[i].seek(destinationSeek,destWeight); //seek the Goal !@#
boids[i].seek(destination,destWeight); //seek the Goal !@#
boids[i].update(boids);
if(boids[i].reachedDestination)
removeBoid(boids[i].loc.x,boids[i].loc.y,1); //ineffcient way to remove boids
}
centroid /=flockSize();
/*
if(sceneMap->getCell(centroid.x,centroid.y))
{
vector<Vec2f> path;
path = pathFinder.getPath(sceneMap,centroid);
if(path.size()>1)
{
destinationSeek = path[min((int)path.size()-1,5)];
}
}
*/
if(flockSize()==0)
return 0;
else
return 1;
}
//@todo: убрать\прокомментить этот феерический высер
void TargetEnergySystem::update(EntityPtr entity)
{
if(!HasCmpt(TargetComponent, entity))
return;
GetCmpt(TargetComponent, target_com, entity);
//If target is null, dissappear
if(!target_com->target.is_set())
{
entity->mark_deleted();
return;
}
EntityPtr target = target_com->target;
GetCmpt(EnergyStorageComponent, enesto, target);
GetCmpt(PositionComponent, target_pos_com, target);
GetCmpt(NodeComponent, target_node_com, target);
GetCmpt(MovementComponent, move_com, entity);
GetCmpt(PositionComponent, pos_com, entity);
//If energy reached target
Vec2f dist = target_pos_com->position - pos_com->position;
bool target_changed = false;
if(dist.length() < 0.01f)
{
//Check, is target enemy tower
if(target_com->target_enemy)
{
GetCmpt(EnergyStorageComponent, target_enesto_cmpt, target_com->target);
target_enesto_cmpt->rem_energy(5);
entity->mark_deleted();
return;
}
//@todo: changing entity position thoughtlessly is VERY dangerous!
// it cost 3 evenings of debug.
// Reposition here break GameLogic entity iterator
// Changing position without changing cell can produce entityPtr
// duplicate on two cells.
// @todo: changing entity position should be on map or gamelogic.
/*pos_com->old_position = pos_com->position;
pos_com->position = target_pos_com->position;
GameEngine::get_data()->logic.getMap()->RepositionEntityToCorrectCell
(entity, pos_com->old_position, pos_com->position);*/
EntityIt it = target_node_com->children.begin();
EntityPtr next_target = target;
float min_energy_balance = 1.0f;
while (it != target_node_com->children.end())
{
GetCmpt(EnergyStorageComponent, enesto_child, (it->get()));
if(enesto_child->balance < min_energy_balance)
{
next_target = (*it);
min_energy_balance = enesto_child->balance;
}
it++;
}
if(min_energy_balance == 1.0) //children are full (or not exist)
{
if(!enesto->is_full())
merge_energy(target, entity);
else
{
// All childs are full and we have reached base tower.
if(!target_node_com->parent.is_set())
{
entity->mark_deleted();
return;
}
target_com->target = target_node_com->parent;
target_changed = true;
}
}
else
{
target_com->target = next_target;
target_changed = true;
}
}
if(target_changed)
{
GetCmpt(PositionComponent, new_target_pos, target_com->target);
dist = new_target_pos->position - pos_com->position;
}
// Energy, moving to enemy target, move unlinear
if (target_com->target_enemy)
{
}
else
{
move_com->speed = dist;
move_com->speed.length(0.5f); //0.5 per sec.
}
}
void SerialTestApp::mouseDrag( MouseEvent event )
{
mousePos = event.getPos();
Vec2f offset = mousePos - getWindowCenter();
float speed = offset.length();
offset.normalize();
float amtLeftWheel = 0;
float amtRightWheel = 0;
// Turning scheme:
// 0-90°
// 0 == other wheel moves forward @ speed
// 90 == other wheel moves backwards @ speed
// Never account for moving backwards.
// Hard left or right is all we can do.
float yRange = (MAX((offset.y*-1), 0.0)*2.0f) - 1.0f; // -1..1
//yRange*=-1;
// Always having one wheel moving forward ensures we're
// driving forward. We can't drive backwards.
if(offset.x < 0){
amtRightWheel = 1;
amtLeftWheel = yRange;// -1..1 //offset.y*-1;
}else{
amtLeftWheel = 1;
amtRightWheel = yRange;// -1..1 //offset.y*-1;
}
// Making the lw / rw amount a function of the speed
const static int MAX_SPEED = 200;
float speedScalar = MIN((speed/(float)MAX_SPEED), 1.0);
amtLeftWheel *= speedScalar;
amtRightWheel *= speedScalar;
int lw = 255+(amtLeftWheel*255); // 0..255..500
int rw = 255+(amtRightWheel*255); // 0..255..500
string directions = "" + boost::lexical_cast<string>((int)lw) + "," +
boost::lexical_cast<string>((int)rw) + ",\n";
console() << directions << "\n";
serial.writeString(directions);
// SIM Arduino code
long val = (lw*(long)1000)+rw;
int rVal = val % 1000;
int lVal = (val - rVal) * 0.001;
int lDirection = lVal >= 255 ? 1 : -1;
int rDirection = rVal >= 255 ? 1 : -1;
int lAbsVal = abs(lVal-255);
int rAbsVal = abs(rVal-255);
console() << "lw : " << lw << " rw: " << rw;
console() << " lAbsVal : " << lAbsVal << " rAbsVal: " << rAbsVal << "\n";
_ardLDir = lDirection;
_ardRDir = rDirection;
_ardLVal = lAbsVal;
_ardRVal = rAbsVal;
}
int improvement(int ind, int molSize, int *rotateAngle, int *edgeLenght, int *vertexNumber, Vec2f *p, bool profi, bool do_dist, float multiplier, int worstVertex) {
Vec2f p1 = p[(worstVertex - 1 + ind) % ind];
Vec2f p2 = p[(worstVertex + 1 + ind) % ind];
float r1 = edgeLenght[(ind + worstVertex - 1) % ind];
float r2 = edgeLenght[(ind + worstVertex) % ind];
float len1 = Vec2f::dist(p1, p[worstVertex]);
float len2 = Vec2f::dist(p2, p[worstVertex]);
float r3 = Vec2f::dist(p1, p2) / sqrt(3.0);
Vec2f p3 = (p1 + p2) / 2;
if (rotateAngle[worstVertex] != 0) {
Vec2f a = (p2 - p1) / sqrt(12.0f);
a.rotate(PI / 2 * rotateAngle[worstVertex]);
p3 += a;
}
else {
p3 = (p1*r1 + p2*r2) / (r1 + r2);
}
float len3 = Vec2f::dist(p3, p[worstVertex]);
if (rotateAngle[worstVertex] == 0) r3 = 0;
//printf("%5.5f %5.5f %5.5f %5.5f\n", len1, len2, len3, r3);
Vec2f newPoint;
float eps = 1e-4;
float eps2 = eps * eps;
if (len1 < eps || len2 < eps || len3 < eps) {
p[worstVertex] = (p1 + p2) / 2.0;
}
else {
float coef1 = (r1 / len1 - 1);
float coef2 = (r2 / len2 - 1);
float coef3 = (r3 / len3 - 1);
if (rotateAngle[worstVertex] != 0) {
float angle = acos(Vec2f::cross(p1 - p[worstVertex], p2 - p[worstVertex]) / (Vec2f::dist(p1, p[worstVertex])*Vec2f::dist(p2, p[worstVertex])));
//if (angle < 2 * PI / 3) coef3 /= 10;
}
//if (!isIntersec(x[worstVertex], y[worstVertex], x3, y3, x1, y1, x2, y2)) coef3 *= 10;
if (rotateAngle[worstVertex] == 0) coef3 = -1;
//printf("%5.5f %5.5f %5.5f\n", coef1, coef2, coef3);
newPoint += (p[worstVertex] - p1)*coef1;
newPoint += (p[worstVertex] - p2)*coef2;
newPoint += (p[worstVertex] - p3)*coef3;
if (do_dist) {
if (profi) {
for (int i = 0; i < ind; i++) if (i != worstVertex && (i + 1) % ind != worstVertex) {
float dist = Vec2f::distPointSegment(p[worstVertex], p[i], p[(i + 1) % ind]);
if (dist < 1 && dist > eps) {
Vec2f normal = (p[(i + 1) % ind] - p[i]);
normal.rotate(PI / 2);
float c = Vec2f::cross(p[i], p[(i + 1) % ind]);
float s = normal.length();
normal /= s;
c /= s;
float t = -c - Vec2f::dot(p[worstVertex], normal);
Vec2f pp;
if (s < eps) {
pp = p[i];
}
else {
pp = p[worstVertex] + normal * t;
if (Vec2f::dist(p[worstVertex], p[i]) < Vec2f::dist(p[worstVertex], pp)) {
pp = p[i];
}
if (Vec2f::dist(p[worstVertex], p[(i + 1) % ind]) < Vec2f::dist(p[worstVertex], pp)) {
pp = p[(i + 1) % ind];
}
}
float coef = (1 - dist) / dist;
newPoint += (p[worstVertex] - pp) * coef;
}
}
}
else {
float good_distance = 1;
for (int j = 0; j < ind; j++) {
int nextj = (j + 1) % ind;
Vec2f pp = p[j];
Vec2f dpp = (p[nextj] - p[j]) / edgeLenght[j];
for (int t = vertexNumber[j], s = 0; t != vertexNumber[nextj]; t = (t + 1) % molSize, s++) {
if (t != vertexNumber[worstVertex] && (t + 1) % molSize != vertexNumber[worstVertex] && t != (vertexNumber[worstVertex] + 1) % molSize) {
float distSqr = Vec2f::distSqr(pp, p[worstVertex]);
if (distSqr < good_distance && distSqr > eps2) {
float dist = sqrt(distSqr);
float coef = (good_distance - dist) / dist;
//printf("%5.5f \n", dist);
newPoint += (p[worstVertex] - pp)*coef;
}
}
pp += dpp;
}
}
}
}
newPoint *= multiplier;
p[worstVertex] += newPoint;
}
return worstVertex;
}
float MapperOp::angle(const Vec2f& v1, const Vec2f& v2)
{
return acos( v1.dot(v2) / (v1.length() * v2.length()) );
}
void cApp::update(){
/*
add particle
*/
int numPs = ps.size();
if( frame%100 != 0 ){
int num = randInt(10, 30);
for( int i=0; i<num; i++ ){
// pick up color
int cid = numPs + i;
int cidx = cid % mColorSample1.size();
int cidy = cid / mColorSample1.size();
cidx %= mColorSample1.size();
cidy %= mColorSample1[0].size();
ColorAf col = mColorSample1[cidx][cidy];
float angle = mPlns[1].fBm( cid*0.01 ) * 10.0;
Vec2f vel = Vec2f(1,1) * randFloat(10, 20);
vel.rotate(angle);
ps.push_back( Vec2f(0,0) );
cs.push_back( col );
vs.push_back( vel );
}
}else{
int num = randInt(20000, 30000);
for( int i=0; i<num; i++ ){
// pick up color
int cid = numPs + i;
int cidx = cid % mColorSample1.size();
int cidy = cid / mColorSample1.size();
cidx %= mColorSample1.size();
cidy %= mColorSample1[0].size();
ColorAf col = mColorSample1[cidx][cidy];
Vec3f n = mPlns[0].dfBm( frame, col.r*col.g, col.b );
Vec2f vel;
vel.x = n.x * n.y * 20;
vel.y = n.y * n.z * 20;
if(vel.length() <= 20){
vel = vel.normalized() * lmap(abs(n.z), 0.0f, 1.0f, 0.7f, 1.0f) *20;
}
ps.push_back( Vec2f(0,0) );
cs.push_back( col );
vs.push_back( vel );
}
}
/*
update particle
*/
for( int i=0; i<ps.size(); i++ ){
Vec2f & p = ps[i];
Vec2f & v = vs[i];
ColorAf & c = cs[i];
Vec3f n = mPlns[1].dfBm( frame*0.01, i*0.01, c.b*0.5 );
c.r += n.x * 0.02;
c.g += n.y * 0.02;
c.b += n.z * 0.02;
Vec2f new_vel;
new_vel.x = n.x;
new_vel.y = n.y;
v += new_vel*2;
p += v;
v *= 0.999;
c.a *= 0.999;
}
/*
remove
*/
vector<Vec2f>::iterator pit = ps.begin();
vector<Vec2f>::iterator vit = vs.begin();
vector<ColorAf>::iterator cit = cs.begin();
int xmin = -mExp.mFbo.getWidth()/2;
int xmax = mExp.mFbo.getWidth()/2;
int ymin = -mExp.mFbo.getHeight()/2;
int ymax = mExp.mFbo.getHeight()/2;
while ( pit!=ps.end() ) {
Vec2f & p = *pit;
if( (p.x<xmin || xmax<p.x) || p.y<ymin || ymax<p.y ){
pit = ps.erase(pit);
vit = vs.erase(vit);
cit = cs.erase(cit);
}else{
pit++;
vit++;
cit++;
}
}
frame++;
}
/*! The mouseMove is called by the viewer when the mouse is moved in the
viewer and this handle is the active one.
\param x the x-pos of the mouse (pixel)
\param y the y-pos of the mouse (pixel)
*/
void Manipulator::mouseMove(const Int16 x,
const Int16 y)
{
//SLOG << "Manipulator::mouseMove() enter\n" << std::flush;
// get the beacon's core (must be ComponentTransform) and it's center
if( getTarget() != NULL )
{
// get transformation of beacon
Transform *t = dynamic_cast<Transform *>(getTarget()->getCore());
if( t != NULL )
{
UInt16 coord(0); // active coordinate: X=0, Y=1, Z=2
Int16 xDiff; // the mousepos x delta
Int16 yDiff; // the mousepos y delta
Vec3f centerV; // center of beacon
Vec3f handleCenterV; // center of subhandle
Vec2f mouseScreenV; // mouse move vector
Vec2f handleScreenV; // handle vec in (cc)
Real32 handleScreenVLen; // len of handle vec in (cc)
Vec3f translation; // for matrix decomposition
Quaternion rotation;
Vec3f scaleFactor;
Quaternion scaleOrientation;
// TODO: das ist ja schon ein wenig haesslich
static const Vec3f coordVector[3] = {
Vec3f(1.0f, 0.0f, 0.0f),
Vec3f(0.0f, 1.0f, 0.0f),
Vec3f(0.0f, 0.0f, 1.0f)
};
// check for the active handle
if( getActiveSubHandle() == getHandleXNode())
{
coord = 0;
}
else if(getActiveSubHandle() == getHandleYNode())
{
coord = 1;
}
else if(getActiveSubHandle() == getHandleZNode())
{
coord = 2;
}
// TODO: only for debugging, -> FDEBUG
//SLOG << "moving " << ( coord == 0 ? "x\n" :
// coord == 1 ? "y\n" :
// "z\n" )
// << std::flush;
// calculate diffs between current and last mouse position
xDiff = x - Int16(getLastMousePos()[0]);
yDiff = y - Int16(getLastMousePos()[1]);
// set the vector resulting from user mouse movement and calc its length
mouseScreenV.setValues(xDiff, -yDiff);
t->getMatrix().getTransform(translation, rotation, scaleFactor,
scaleOrientation);
// calculate the camera coordinate of the center
centerV = translation;
Pnt2f centerPixPos = calcScreenProjection(centerV.addToZero(),
getViewport());
// calculate the camera coordinate of the handle center
handleCenterV = centerV + coordVector[coord]*getLength()[coord];
Pnt2f handleCenterPixPos = calcScreenProjection(handleCenterV.addToZero(),
getViewport());
handleScreenV = handleCenterPixPos - centerPixPos;
handleScreenVLen = handleScreenV.length();
Real32 s = handleScreenV.dot(mouseScreenV) / handleScreenVLen;
doMovement(t, coord, s * getLength()[coord] * 0.01, translation,
rotation, scaleFactor, scaleOrientation);
}
else
{
SWARNING << "handled object has no parent transform!\n";
}
callExternalUpdateHandler();
}
else
{
SWARNING << "Handle has no target.\n";
}
setLastMousePos(Pnt2f(Real32(x), Real32(y)));
updateHandleTransform();
//SLOG << "Manipulator::mouseMove() leave\n" << std::flush;
}
