void SpaceObject::matchVelocity(std::string objectName) {
Object *o = ObjectMgr::get().find(objectName);
if (o && objectName != id) {
Vector deltaV = o->getVelocity() - velocity;
if (deltaV.magnitude() > 0.1f) {
Vector thrustDir = deltaV.normalize();
Vector axis = (look % thrustDir).normalize();
if (axis.magnitude() < 0.01f)
axis = up;
float ang = look.angle(thrustDir);
if (ang < 0.01f || fabs(ang - 180.0f) < 0.1f)
axis = up;
if (ang > 0.0001f) {
if (ang > topTurnSpeed * GlobalTime::get().frameSec)
ang = topTurnSpeed * GlobalTime::get().frameSec;
rotateAxis(axis, ang);
}
thrust(100.0f);
}
}
}
void Ant::step(){
std::cout << "Ant #" << agentID << " making internal step.\n";
Vector force = getSteeringForce();
// f / m = a
Vector newAcceleration = (force / _mass);
// get velocity
Vector newVelocity = _velocity;
std::cout << "velocity: "; newVelocity.print(); std::cout << "\n";
std::cout << "acceleration: "; newAcceleration.print(); std::cout << "\n";
// change the velocity by the acceleration * delta of time
int elapsedTime = stepClock.restart().asSeconds();
std::cout << "elapsed: " << elapsedTime << "\n";
newVelocity = newVelocity + (newAcceleration * elapsedTime);
std::cout << "velocity: "; newVelocity.print(); std::cout << "\n";
// set velocity and speed
setSpeed(newVelocity.magnitude());
if(_speed > _maxSpeed)
{
newVelocity = newVelocity / newVelocity.magnitude();
newVelocity = newVelocity * _maxSpeed;
setSpeed(_maxSpeed);
}
constrainToSurface(newVelocity);
setVelocity(newVelocity);
setPosition (position + (newVelocity * elapsedTime));
sprite.setPosition(position.x, position.y);
flags.insert(AgentFlags::moved);
//std::cout << "Step finished!\n";
}
Vector Vector::normalize() {
Vector normal = *this;
float mag = (normal.magnitude()==0)?1:normal.magnitude();
normal.div(mag);
return normal;
}
void move() {
for(unsigned int i=0; i<birds2.size(); i++) {
Vector desired = Vector();
Vector cohesion = birds2[i].cohesion(birds1);
Vector chill = birds2[i].chill(birds1, predators1);
desired.add(chill);
if(desired.magnitude() == 0) {
Vector wander = birds1[i].wander();
desired.add(wander);
}
desired.truncate(birds1[i].getMaxA());
// Euler forward integration
Vector velocity = birds1[i].getVel();
velocity.add(desired);
velocity = birds2[i].matchTurn(birds1[i].getVel(), velocity);
velocity.truncate(birds1[i].getMaxV());
birds2[i].setVel(velocity);
Vector position = birds1[i].getPos();
position.add(velocity);
birds2[i].setPos(position);
birds2[i].keepBound(world);
//birds2[i].wrapAround(world);
}
for(unsigned int j=0; j<predators2.size(); j++) {
Vector desired = Vector();
Vector hunt = predators2[j].hunt(birds1);
desired.add(hunt);
if(desired.magnitude() == 0) {
Vector wander = predators2[j].wander();
desired.add(wander);
}
desired.truncate(predators1[j].getMaxA());
// Euler forward integration
Vector velocity = predators1[j].getVel();
velocity.add(desired);
velocity = predators2[j].matchTurn(predators1[j].getVel(), velocity);
velocity.truncate(predators1[j].getMaxV());
predators2[j].setVel(velocity);
Vector position = predators1[j].getPos();
position.add(velocity);
predators2[j].setPos(position);
predators2[j].keepBound(world);
//birds2[i].wrapAround(world);
}
}
RotateAndScaleCoefs Procrustes2D::align(const Vector &from, const Vector &to)
{
double referenceScale = 1.0/to.magnitude();
Vector reference = to.mul(referenceScale);
double vectorScale = 1.0/from.magnitude();
Vector vector = from.mul(vectorScale);
double s = vectorScale/referenceScale;
double theta = getOptimalRotation(vector, reference);
RotateAndScaleCoefs c(s, theta);
return c;
}
void SpaceObject::pointTowards(std::string objectName) {
Object *o = ObjectMgr::get().find(objectName);
if (o && objectName != id) {
Vector delta = o->getPos() - pos;
delta = delta.normalize();
Vector axis = (delta % look).normalize();
//std::cerr<<"youch!!!!\n";
if (axis.magnitude() < 0.01f)
axis = up;
float ang = look.angle(delta);
if (ang < 0.01f || fabs(ang - 180.0f) < 0.1f)
axis = up;
if (ang > 0.0001f
|| (look.normalize() - delta.normalize()).magnitude() > 1.0f) {
if (ang > topTurnSpeed * GlobalTime::get().frameSec)
ang = topTurnSpeed * GlobalTime::get().frameSec;
rotateAxis(axis, ang);
}
}
}
Vector ClosestPointCylinder ( Vector const & V, Cylinder const & C )
{
Vector delta = V - C.getBase();
delta.y = 0.0f;
real dist = delta.magnitude();
Vector point = V;
// clamp the x-z coords of the point so they're inside the tube
IGNORE_RETURN( delta.normalize() );
delta *= std::min( C.getRadius(), dist );
point = C.getBase() + delta;
// and clamp the y coord so it's inside also
real min = C.getBase().y;
real max = min + C.getHeight();
point.y = clamp( min, V.y, max );
return point;
}
float PressureKernel::operator() (const Vector& v) const {
float r = v.magnitude();
if (r >= h) {
return 0.0f;
}
return (15.0f / (M_PI * pow(h, 6.0f))) * pow(h - r, 3.0f);
}
Vector Bird::matchTurn(Vector init_speed, Vector desired_speed) {
float velocity_angle = init_speed.angle(desired_speed);
if(velocity_angle>this->max_turn) {
float t = this->max_turn / velocity_angle;
Vector allowed_velocity = init_speed;
allowed_velocity.mul(1.0f-t);
Vector desired_velocity = desired_speed;
desired_velocity.mul(1.0f-t);
allowed_velocity.add(desired_velocity);
float mag1 = desired_speed.magnitude();
float mag2 = allowed_velocity.magnitude();
if(mag2>0) {
float scale_factor = mag1 / mag2;
allowed_velocity.mul(scale_factor);
}
return allowed_velocity;
}
return desired_speed;
}
float ViscosityKernel::laplacian(const Vector& v) const {
float r = v.magnitude();
if (r >= h) {
return 0.0f;
}
return (45.0f / (M_PI * pow(h, 6.0f))) * (h - r);
}
//
// Behavior avoidBehavior(target, dist)
// Last modified: 07Nov2009
//
// Directs the robot to avoid the parameterized target,
// maintaining the parameterized distance,
// returning the appropriate robot behavior.
//
// Returns: the appropriate robot behavior
// Parameters:
// target in/out the target to avoid
// dist in the distance to maintain from the target
//
Behavior Robot::avoidBehavior(const Vector &target, const GLfloat dist)
{
GLfloat r = target.magnitude();
if (r < dist) return sumBehaviors(orientToBehavior(target, 180.0f),
moveForwardBehavior(dist - r));
return moveStopBehavior();
} // avoidBehavior(const Vector &, const GLfloat)
bool Object::detectCollision(Object *other) {
if (other->getSource() != id) {
long curFrame = GlobalTime::get().getFrames();
std::string otherID = other->getSource();
if (otherID == collisionID || lastCollision != curFrame) {
Vector v = other->pos - pos;
if (v.magnitude()
< (((size * scale) / 2) + ((other->size * other->scale) / 2))) {
/*C3ds *mod = (C3ds*)ModelMgr::get().find(model);
C3ds *mod2 = (C3ds*)ModelMgr::get().find(other->model);
if(mod && mod2)
{
return mod->intersect(mod2,scale,other->scale, (FVector){v.x,v.y,v.z},q,other->q);
}
else
{
return true;
}*/
return true;
}
//if(sqrt((pos.x-other->pos.x)*(pos.x-other->pos.x)+(pos.y-other->pos.y)(pos.y-other->pos.y))<((size/2)+(other->size/2))) return true;
collisionID = otherID;
lastCollision = curFrame;
}
}
return false;
}
/*
\fn calculate the projected vector of this onto vec
\param vec the vector to be projected onto
\return
\li the project vector of this onto vec
*/
Vector projectedVector(const Vector& vec1, const Vector& vec2){
real dotProd = CapEngine::dotProduct(vec1, vec2);
real magVec = vec2.magnitude();
Vector v(vec2);
v.scale( dotProd / (magVec * magVec) );
return v;
}
//NOTE - NEED TO FIX LIKE WITH STOP AND MATCHVELOCITY
//180* BUG
void SpaceObject::track(std::string objectName) {
Object *o = ObjectMgr::get().find(objectName);
if (o && objectName != id) {
//Vector from your position to object being tracked
Vector delta = o->getPos() - pos;
float seconds = ((-1 * (velocity - o->getVelocity()).magnitude()
+ sqrt(
(velocity - o->getVelocity()).magnitude()
* (velocity - o->getVelocity()).magnitude()
+ 2 * topAccel * delta.magnitude())) / (topAccel));
float seconds2 = ((-1 * (velocity - o->getVelocity()).magnitude()
- sqrt(
(velocity - o->getVelocity()).magnitude()
* (velocity - o->getVelocity()).magnitude()
+ 2 * topAccel * delta.magnitude())) / (topAccel));
if (seconds2 > seconds)
seconds = seconds2;
//float seconds = sqrt((delta.magnitude()*2.0f)/topAccel);
//Distance 1s from now
Vector shootFor = delta + (o->getVelocity() - velocity) * seconds;
shootFor = shootFor.normalize();
Vector axis = (look % shootFor).normalize();
//std::cerr<<"youch!!!!\n";
if (axis.magnitude() < 0.01f)
axis = up;
float ang = look.angle(shootFor);
if (ang < 0.01f || fabs(ang - 180.0f) < 0.1f)
axis = up;
if (ang > 0.0001f) {
if (ang > topTurnSpeed * GlobalTime::get().frameSec)
ang = topTurnSpeed * GlobalTime::get().frameSec;
rotateAxis(axis, ang);
}
thrust(100.0f);
//if(detectCollision(o)) done=true;
}
}
float Vector::angle(Vector vec) {
float angle;
float mag1 = this->magnitude();
float mag2 = vec.magnitude();
float dot_product = this->dotproduct(vec);
return angle = acosf(ceilf(dot_product/(mag1*mag2))) * 180 / PI;
}
float ViscosityKernel::operator() (const Vector& v) const {
float r = v.magnitude();
if (r >= h) {
return 0.0f;
}
float rh = r / h;
return (15.0f / (2.0f * M_PI * pow(h, 3.0f))) * (-0.5f*pow(rh, 3.0f) + pow(rh, 2.0f) + pow(2.0f*rh, -1.0f) - 1.0f);
}
Vector PressureKernel::gradient(const Vector& v) const {
float r = v.magnitude();
if (r >= h) {
return 0.0f;
}
float t = (-45.0f / (M_PI * pow(h, 6.0f))) * pow(h - r, 2.0f);
return (v.normalize() * t);
}
//
// Behavior followBehavior(target, dist)
// Last modified: 07Nov2009
//
// Directs the robot to follow the parameterized target
// maintaining the parameterized distance,
// returning the appropriate robot behavior.
//
// Returns: the appropriate robot behavior
// Parameters:
// target in/out the target to follow
// dist in the distance to maintain from the target
//
Behavior Robot::followBehavior(const Vector &target, const GLfloat dist)
{
GLfloat r = target.magnitude();
if (r <= threshold()) return moveStopBehavior();
Behavior beh = orientToBehavior(target, 0.0f);
if ((beh.isDone()) && (r > dist)) return moveForwardBehavior(r - dist);
return beh;
} // followBehavior(const Vector &, const GLfloat)
bool CollisionCallbackManager::intersectAndReflectWithTerrain(Object * const object, Result & result)
{
CollisionProperty const * collision = object->getCollisionProperty();
NOT_NULL(collision);
Capsule queryCapsule_w(collision->getQueryCapsule_w());
float const radius = queryCapsule_w.getRadius();
Vector const deltaTraveled_w(queryCapsule_w.getDelta());
Vector direction_w(deltaTraveled_w);
if (direction_w.normalize())
{
Vector const begin_w(queryCapsule_w.getPointA());
Vector const end_w(queryCapsule_w.getPointB() + direction_w * radius);
DEBUG_REPORT_LOG(ms_debugReport, ("terrain begin = %f %f %f\n", begin_w.x, begin_w.y, begin_w.z));
DEBUG_REPORT_LOG(ms_debugReport, ("terrain end = %f %f %f\n", end_w.x, end_w.y, end_w.z));
DEBUG_REPORT_LOG(ms_debugReport, ("terrain direction = %f %f %f\n", direction_w.x, direction_w.y, direction_w.z));
DEBUG_REPORT_LOG(ms_debugReport, ("terrain length = %f\n", deltaTraveled_w.magnitude()));
TerrainObject const * const terrainObject = TerrainObject::getConstInstance();
if (terrainObject != 0)
{
CollisionInfo info;
if (terrainObject->collide (begin_w, end_w, info))
{
#ifdef _DEBUG
// calculate the parametric time for logging
float const actualDistance = begin_w.magnitudeBetween(info.getPoint());
float const attemptedDistance = begin_w.magnitudeBetween(end_w);
float const parametricTime = (attemptedDistance != 0.0f) ? (actualDistance / attemptedDistance) : 0.0f;
#endif
Vector const & pointOfCollision_w = info.getPoint();
#ifdef _DEBUG
DEBUG_REPORT_LOG(ms_debugReport, ("\t\tterrain HIT!\n"));
DEBUG_REPORT_LOG(ms_debugReport, ("\t\tterrain time = %f\n", parametricTime));
DEBUG_REPORT_LOG(ms_debugReport, ("\t\tterrain point = %f %f %f\n", pointOfCollision_w.x, pointOfCollision_w.y, pointOfCollision_w.z));
#endif
result.m_pointOfCollision_p = pointOfCollision_w;
result.m_normalOfSurface_p = info.getNormal();
result.m_deltaToMoveBack_p = pointOfCollision_w - end_w;
result.m_newReflection_p = result.m_normalOfSurface_p.reflectIncoming(direction_w);
#ifdef _DEBUG
DEBUG_REPORT_LOG(ms_debugReport, ("\t\tterrain = %f %f %f\n", result.m_deltaToMoveBack_p.x, result.m_deltaToMoveBack_p.y, result.m_deltaToMoveBack_p.z));
#endif
return true;
}
}
}
return false;
}
float PressureKernel::laplacian(const Vector& v) const {
float r = v.magnitude();
if (r <= 0.0f || r >= h) {
return 0.0f;
}
float t = (-45.0f / (M_PI * pow(h, 6.0f)));
float hr = h - r;
return (t * hr * (3.0f / r - hr * r + 2.0f));
}
//
// Behavior orientForOrbitBehavior(target, dist)
// Last modified: 07Nov2009
//
// Orients the robot with respect to the parameterized target
// such that if the robot were moving forward, it would move
// in a circular path maintaining the parameterized distance
// around the target, returning the appropriate robot behavior.
//
// Returns: the appropriate robot behavior
// Parameters:
// target in/out the target to orient to for orbit
// dist in the distance to maintain from the target
//
Behavior Robot::orientForOrbitBehavior(const Vector &target,
const GLfloat dist)
{
GLfloat r = target.magnitude(), dir = 0.0f;
if (r > dist + collisionRadius()) dir = 0.0f;
else if (r < dist - collisionRadius()) dir = 180.0f;
else dir = 180.0f - r * 90.0f / collisionRadius();
return orientToBehavior(target, dir);
} // orientForOrbitBehavior(const Vector &, const GLfloat)
void Character::updatePosition ( )
{
if ( velocity_.magnitude() > maxSpeed_ ) {
velocity_.normalise();
velocity_ *= maxSpeed_;
}
position_ += velocity_ / SCREEN_FRAMERATE;
}
void Object::angularMove() {
Vector dt1 = omega * (GlobalTime::get().frameSec);
Vector dt2 = (omega + dt1 * .5) * (GlobalTime::get().frameSec);
Vector dt3 = (omega + dt2 * .5) * (GlobalTime::get().frameSec);
Vector dt4 = (omega + dt3) * (GlobalTime::get().frameSec);
//pos+=velocity*(GlobalTime::get().frameSec);
Vector incr = (dt1 / 6 + dt2 / 3 + dt3 / 3 + dt4 / 6);
rotateAxis(incr.normalize(), RAD2DEG(incr.magnitude()));
alpha *= 0;
}
void Ball :: collide()
{
float x,y;
float ballx = ballpos.xcomp;
float bally = ballpos.ycomp;
if(bally <= 0)
ballspeed.ycomp = -ballspeed.ycomp;
if(bally >= HEIGHT)
{
ballspeed.ycomp = -ballspeed.ycomp;
glutPostRedisplay();
}
if(ballx <= 0 || ballx >= WIDTH)
{
ballspeed.xcomp = -ballspeed.xcomp;
glutPostRedisplay();
}
for(x=0;x<WIDTH*FLOORRES;x++)
{
y = floorval[(int)x];
if(ballx == x && bally == y)
glutPostRedisplay();
Vector p;
p.setvals(x,y);
p = ballpos - p;
if(/*(x-ballx)*(x-ballx)+(y-bally)*(y-bally)*/ p.magnitude() <= BALLRADIUS/*BALLRADIUS*/)
{
//p = ballpos - p;
/*#ifdef DEBUG
char log[512];
sprintf(log, "%f: %f %f %f %f \n",currentTime/1000.0, ballpos.xcomp, ballpos.ycomp, ballspeed.xcomp, ballspeed.ycomp);
write(fd, log, strlen(log));
#endif // DEBUG*/
ballspeed = (p/p.magnitude()) * ballspeed.magnitude();
}
}
glutPostRedisplay();
}
Vector ViscosityKernel::gradient(const Vector& v) const {
float r = v.magnitude();
if (r <= 0.0f || r >= h) {
return 0.0f;
}
float h2 = h * h;
float a1 = -3.0f * r / (h2 * h2);
float a2 = 1.0f / (h2 * h);
float a3 = -0.5f / (r * r * r);
float t = (15.0f / (2.0f * M_PI * h2)) * (a1 + a2 + a3);
return v * t;
}
Range ProjectAxis ( Line3d const & L, OrientedCylinder const & C )
{
Vector const & N = L.getNormal();
Vector cross = C.getAxis().cross(N);
float sine = cross.magnitude() / N.magnitude();
real d = sine * C.getRadius();
Range c = ProjectAxis( L, C.getAxisSegment() );
return Range( c.getMin() - d, c.getMax() + d );
}
unsigned int ShipClientUpdateTracker::getShipUpdatePriorityValue(Client const &client, ShipObject const &ship) // static
{
CreatureObject const * const characterObject = safe_cast<CreatureObject const *>(client.getCharacterObject());
if (characterObject && characterObject->getLookAtTarget() != ship.getNetworkId())
{
Object const * const clientTopmost = NON_NULL(ContainerInterface::getTopmostContainer(*characterObject));
Vector const shipOffset = ship.getPosition_p()-clientTopmost->getPosition_p();
float const distance = shipOffset.magnitude();
float const angle = acosf(clientTopmost->getObjectFrameK_p().dot(shipOffset/distance));
float const anglePriorityAdjust = 1.f+((clamp(PI_OVER_4, angle, PI)-PI_OVER_4)/(PI-PI_OVER_4))*4.f; // 1 when roughly in view frustum, up to 5 directly behind
return static_cast<unsigned int>(clamp(100.f, distance, 8192.f)*anglePriorityAdjust);
}
return 100;
}
Range ProjectAxis ( Line3d const & L, OrientedCircle const & S )
{
Vector const & N = L.getNormal();
Vector cross = S.getAxis().cross(N);
float sine = cross.magnitude() / N.magnitude();
real d = sine * S.getRadius();
real c = ProjectAxis( L, S.getCenter() );
return Range( c - d, c + d );
}
ContainmentResult TestPointCylinder ( Vector const & V,
Cylinder const & C )
{
Vector delta = V - C.getBase();
delta.y = 0;
real dist = delta.magnitude();
real radius = C.getRadius();
ContainmentResult rTest = Containment1d::TestFloatLess(dist,radius);
ContainmentResult vTest = Containment1d::TestFloatRange(V.y, C.getRangeY());
// ----------
return Containment::ComposeAxisTests(rTest,vTest);
}
//
// GLfloat rangeSensor(p)
// Last modified: 08Nov2009
//
// Returns with the distance from this robot
// to the position being auctioned.
//
// Returns: the distance from this robot to the position being auctioned
// Parameters:
// p in/out the packet (auction) to be processed
GLfloat Robot::rangeSensor(Packet &p)
{
// unpack the auction from the packet
Push_Auction_Announcement *aa = (Push_Auction_Announcement *)p.msg;
// unpack the formation definition from the state within the auction
Formation f = aa->s_i.formation;
// get the range from the auctioneer to this robot
GLfloat range = env->distanceToRobot(env->getCell(p.fromID), this);
// if the auctioneer is beyond sensor range, do not bid
if (range > SENSOR_RANGE) return -1.0f;
// get the relative bearing from the auctioneer to this robot
GLfloat bearing = bearingSensor(p.fromID);
// get the angle between the auctioneer and the position being auctioned
GLfloat ang = atan2(f.getFunction()(f.getRadius()), f.getRadius()) -
bearing;
// get the vector between the auctioneer and this robot
Vector d = (*(Vector *)this) - (*(Vector *)env->getCell(p.fromID));
// get the vector between the auctioneer and the position being auctioned
Vector e;
e.y = f.getFunction()(f.getRadius());
e.x = sqrt((f.getRadius() * f.getRadius()) - (e.y * e.y));
// get the vector between the position being auctioned and this robot
Vector a;
if (aa->right) // if the position is to the right of the auctioneer
a = d - e;
else
a = d + e;
// range is the magnitude of the vector between
// the position being auctioned and this robot
range = a.magnitude();
//cout << "robot " << getID() << " bidding " << range << endl;
return range;
} // rangeSensor(Packet &)
