// Sphere Plane Collision - returns true if collsion exists - takes Point3d params
bool Collision::SpherePlane(Vector3d& centerSphere, float radiusSphere,
Vector3d planeNormal, Point3d p1, Point3d p2, Point3d p3, Point3d p4){
float dist1 = 0.0;
float dist2 = 0.0;
// this checks for the collsion on both sides of the plane
if(RayPlane(planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(), p1.getX(), p1.getY(), p1.getZ(),
centerSphere.getX(), centerSphere.getY(), centerSphere.getZ(), -planeNormal.getX(), -planeNormal.getY(), -planeNormal.getZ(),
p1, p2, p3, p4, &dist1) || RayPlane(-planeNormal.getX(), -planeNormal.getY(), -planeNormal.getZ(), p1.getX(), p1.getY(), p1.getZ(),
centerSphere.getX(), centerSphere.getY(), centerSphere.getZ(), planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(),
p1, p2, p3, p4, &dist2)){
if(dist1 > radiusSphere || dist2 > radiusSphere){ // if either distance is > the radiusSphere....no collision
return false;
}
// indicates that we are on the side where dist1 has been determined to be > 0
// the else is the other side of the plane. This calculates the point as well
// as the new location of the sphere
if(dist1 > 0 ){
centerSphere.setX(centerSphere.getX() + planeNormal.getX() * (radiusSphere - dist1));
centerSphere.setY(centerSphere.getY() + planeNormal.getY() * (radiusSphere - dist1));
centerSphere.setZ(centerSphere.getZ() + planeNormal.getZ() * (radiusSphere - dist1));
}else{
centerSphere.setX(centerSphere.getX() + planeNormal.getX() * (radiusSphere - dist2));
centerSphere.setY(centerSphere.getY() + planeNormal.getY() * (radiusSphere - dist2));
centerSphere.setZ(centerSphere.getZ() + planeNormal.getZ() * (radiusSphere - dist2));
}
return true;
}
return false;
}
bool Box::isInterior(const Vector3d &point) const
{
const bool xIsInterior = ( minX < point.getX() && point.getX() < maxX );
const bool yIsInterior = ( minY < point.getY() && point.getY() < maxY );
const bool zIsInterior = ( minZ < point.getZ() && point.getZ() < maxZ );
return xIsInterior && yIsInterior && zIsInterior;
}
float Vector3d::inverseVectorDistanceSquared(Vector3d v1, Vector3d v2){
Vector3d tmpVector(v1.getX() - v2.getX(),
v1.getY() - v2.getY(),
v1.getZ() - v2.getZ());
return(tmpVector.getX() * tmpVector.getX() +
tmpVector.getY() * tmpVector.getY() +
tmpVector.getZ() * tmpVector.getZ());
}
Box::Box(const Vector3d& pointX, const Vector3d& pointY) :
maxX( (pointX.getX() > pointY.getX() ) ? pointX.getX() : pointY.getX() ),
minX( (pointX.getX() < pointY.getX() ) ? pointX.getX() : pointY.getX() ),
maxY( (pointX.getY() > pointY.getY() ) ? pointX.getY() : pointY.getY() ),
minY( (pointX.getY() < pointY.getY() ) ? pointX.getY() : pointY.getY() ),
maxZ( (pointX.getZ() > pointY.getZ() ) ? pointX.getZ() : pointY.getZ() ),
minZ( (pointX.getZ() < pointY.getZ() ) ? pointX.getZ() : pointY.getZ() )
{}
void Physics::ballVelocityFunc(cpBody* ipBody, cpVect iGravity,
cpFloat iDamping, cpFloat iDt)
{
cpBodyUpdateVelocity(ipBody, iGravity, iDamping, iDt);
double kw = 150; //vitesse maximal angulaire qui peut induire une deviation
//influence angulaire normalisé
double nw = min(kw, fabs(ipBody->w)) / kw;
double kv = 60;
double nv = min(kv, cpvlength(ipBody->v)) / kv;
double d = 0.03; //magnitude maximale du vecteur de déviation
//trouvons le vecteur laterale a la velocity
Vector3d lat =
Vector3d(ipBody->v.x, ipBody->v.y, 0.0) ^ Vector3d(0.0, 0.0, 1.0);
lat.normalise();
cpVect cpLat = cpv((cpFloat)lat.getX(), (cpFloat)lat.getY());
cpLat = ipBody->w <= 0 ? cpLat : cpvmult(cpLat, -1.0);
ipBody->v = cpvadd(ipBody->v, cpvmult(cpLat, nw * d));
// //faire decroitre la rotation angulaire
// if(abs(ipBody->w) >= 20.0)
// ipBody->w += iDt * 20.0 * ipBody->w >= 0.0 ? -1 : 1;
// cout << "dt: " << iDt << endl;
// cout << "Angular velocity: " << ipBody->w << endl;
// cout << "velocity: " << cpvlength(ipBody->v) << endl;
// cout << "velocity vect: " << ipBody->v.x << " " << ipBody->v.y << endl;
// cout << "velocity lat: " << cpLat.x << " " << cpLat.y << endl;
// cout << "Influence angulaire normalisé: " << nw << endl;
// cout << "Influence vitesse normalisé: " << nv << endl;
// cout << "Influence sur la déviation: " << nw * nv << endl << endl;
}
/**
this method returns a vector that is the current vector, rotated by an angle alpha (in radians) around the axis given as parameter.
IT IS ASSUMED THAT THE VECTOR PASSED IN IS A UNIT VECTOR!!!
*/
Vector3d Vector3d::rotate(double alpha, const Vector3d &axis){
//ok... this is fairly hard - check the red book for details.
double xP = axis.getX();
double yP = axis.getY();
double zP = axis.getZ();
double cosa = cos(alpha);
double sina = sin(alpha);
double s[3][3] = {{0, -zP, yP},
{zP, 0, -xP},
{-yP, xP, 0}};
double UUT[3][3] = { {xP*xP, xP*yP, xP*zP},
{yP*xP, yP*yP, yP*zP},
{zP*xP, zP*yP, zP*zP}};
double I[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
double R[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
for (int i=0;i<3;i++)
for (int j = 0;j<3;j++)
R[i][j] = UUT[i][j] + cosa*(I[i][j] - UUT[i][j]) + sina*s[i][j];
//now that we finally have the transformation matrix set up, we can rotate the vector
Vector3d result;
result.setX(R[0][0]*x + R[0][1]*y + R[0][2]*z);
result.setY(R[1][0]*x + R[1][1]*y + R[1][2]*z);
result.setZ(R[2][0]*x + R[2][1]*y + R[2][2]*z);
return result;
}
void CartWheel3D::addBall(const string& name, const Vector3d& scale, double mass, const Vector3d& color) {
string mesh = _path + "data/models/sphere10x.obj";
Vector3d offset = Vector3d(0, 0, 0);
#if 1
RigidBody* body = new ArticulatedRigidBody();
#else
RigidBody* body = new RigidBody();
#endif
body->setName(name.c_str());
body->setScale(scale);
body->addMeshObj(mesh.c_str(), offset, scale);
body->setColour(color.x, color.y, color.z, 1);
// body->setColour(0.1, 0, 0.8, 1);
body->setMass(mass);
body->setMOI(Vector3d(0.2, 0.2, 0.2));
Point3d center = Point3d(0, 0, 0);
SphereCDP* sphereCDP = new SphereCDP(center, scale.getX(), body);
body->addCollisionDetectionPrimitive(sphereCDP);
body->setFrictionCoefficient(1.8);
body->setRestitutionCoefficient(0.35);
ArticulatedRigidBody* arb = dynamic_cast<ArticulatedRigidBody*> (body);
if (NULL != arb) {
arb->setAFParent(NULL);
arb->setParentJoint(NULL);
_world->addArticulatedRigidBody(arb);
} else {
_world->addRigidBody(body);
}
}
Ellipsoid<T>::Ellipsoid(const Vector3d<T>& center, const Vector3d<T>& radii) :
IPrimitive3d<T>(center),
radii( radii )
{
assert(radii.getX() > T{ 0 });
assert(radii.getY() > T{ 0 });
assert(radii.getZ() > T{ 0 });
}
Quaternion<T>::Quaternion(const Vector3d<T>& axis, const T angle) :
x(axis.getX() * sin(angle * 0.5f)),
y(axis.getY() * sin(angle * 0.5f)),
z(axis.getZ() * sin(angle * 0.5f)),
w(cos(angle * 0.5f))
{
assert(axis.isNormalized());
}
void GameWindow::addCollision(const Vector3d& p, const Vector3d& n)
{
Collision* c = 0;
for(unsigned int i = 0; i < cMaxCollisions; ++i)
if(mCollisions[i]->isDone())
{ c = mCollisions[i]; break; }
if(c)
{
Matrix4d m;
m.setTranslation(toPoint(p));
c->setTransformationToGlobal(m);
double a = atan2(n.getY(), n.getX());
c->setAngle(a);
c->setNormal(n);
c->animate();
}
}
Vector3d Matrix::matrixMulVec(const Matrix& mat, const Vector3d& vec){
Vector3d noTranslate = matrixMulDir(mat,vec);
return Vector3d( noTranslate.getX()+mat(0,3),
noTranslate.getY()+mat(1,3),
noTranslate.getZ()+mat(2,3));
}
// subtracts a vector from this one and returns a new one
Vector3d Vector3d::operator - (const Vector3d& vector){
return (Vector3d(this->getX() - vector.getX(), this->getY() - vector.getY(), this->getZ() - vector.getZ()));
}
void Vector3d::subtract( Vector3d v ) {
this->x -= v.getX();
this->y -= v.getY();
this->z -= v.getZ();
}
// gets the area of a triangle using the Heron formula
float Collision::triangleArea(Vector3d p1, Vector3d p2, Vector3d p3){
// get the length of the triagle's sides....a,b, and c
float a = sqrt((p2.getX() - p1.getX()) * (p2.getX() - p1.getX()) +
(p2.getY() - p1.getY()) * (p2.getY() - p1.getY()) +
(p2.getZ() - p1.getZ()) * (p2.getZ() - p1.getZ()));
float b = sqrt((p3.getX() - p2.getX()) * (p3.getX() - p2.getX()) +
(p3.getY() - p2.getY()) * (p3.getY() - p2.getY()) +
(p3.getZ() - p2.getZ()) * (p3.getZ() - p2.getZ()));
float c = sqrt((p3.getX() - p1.getX()) * (p3.getX() - p1.getX()) +
(p3.getY() - p1.getY()) * (p3.getY() - p1.getY()) +
(p3.getZ() - p1.getZ()) * (p3.getZ() - p1.getZ()));
// triangle's semi-parameter s
float s = (a + b + c) / 2;
return (sqrt(s * ((s - a) * (s - b) * (s - c))));
}
float getAngleAccelerationZ( float x1, float x2, float x3, const Vector3d& I, const Vector3d& N)
{
return ( N.getZ() + ( I.getX() - I.getY() ) * x1 * x2 ) / I.getZ() - 10.0f * x3;
}
bool Move::doOrder(IGObject * ob){
Vector3d dest = coords->getPosition();
Fleet* fleet = ((Fleet*)(ob->getObjectBehaviour()));
unsigned long long distance = dest.getDistance(fleet->getPosition());
unsigned long long max_speed = 3000000;
Logger::getLogger()->debug("Moving %lld at %lld speed", distance, max_speed);
if(distance <= max_speed){
uint32_t parentid;
Logger::getLogger()->debug("Object(%d)->Move->doOrder(): Is arriving at [%lld, %lld, %lld] ",
ob->getID(), dest.getX(), dest.getY(), dest.getZ());
fleet->setVelocity(Vector3d(0,0,0));
parentid = ob->getParent();
// recontainerise if necessary
int containertype = Game::getGame()->getObjectManager()->getObject(parentid)->getContainerType();
Game::getGame()->getObjectManager()->doneWithObject(parentid);
if(fleet->getPosition() != dest && containertype >= 1){
//removeFromParent();
std::set<unsigned int> oblist = ((MTSecTurn*)(Game::getGame()->getTurnProcess()))->getContainerIds();
for(std::set<unsigned int>::reverse_iterator itcurr = oblist.rbegin(); itcurr != oblist.rend(); ++itcurr){
IGObject* testedobject = Game::getGame()->getObjectManager()->getObject(*itcurr);
Position3dObjectParam * pos = dynamic_cast<Position3dObjectParam*>(testedobject->getParameterByType(obpT_Position_3D));
SizeObjectParam * size = dynamic_cast<SizeObjectParam*>(testedobject->getParameterByType(obpT_Size));
if(pos == NULL || size == NULL){
Game::getGame()->getObjectManager()->doneWithObject(*itcurr);
continue;
}
Vector3d pos1 = pos->getPosition();
uint64_t size1 = size->getSize();
uint64_t diff = dest.getDistance(pos1);
if(diff <= fleet->getSize() / 2 + size1 / 2){
Logger::getLogger()->debug("Container object %d", *itcurr);
//if(Game::getGame()->getObject(*itcurr)->getType() <= 2){
//if(*itcurr != id){
if(size1 >= fleet->getSize()){
if(*itcurr != parentid){
ob->removeFromParent();
ob->addToParent(*itcurr);
}
Game::getGame()->getObjectManager()->doneWithObject(*itcurr);
parentid = *itcurr;
break;
}
}
Game::getGame()->getObjectManager()->doneWithObject(*itcurr);
//}
}
if(parentid == 0){
ob->removeFromParent();
ob->addToParent(0);
}
}
fleet->setPosition(dest);
Message * msg = new Message();
msg->setSubject("Move order complete");
msg->setBody("The fleet '" + ob->getName() + "' has reached it's destination.");
msg->addReference(rst_Action_Order, rsorav_Completion);
msg->addReference(rst_Object, ob->getID());
msg->addReference(rst_Object, parentid); /* It's parent */
Game::getGame()->getPlayerManager()->getPlayer(fleet->getOwner())->postToBoard(msg);
return true;
}else{
Vector3d velo = (dest - fleet->getPosition()).makeLength(max_speed);
Vector3d arriveat = fleet->getPosition()+velo;
Logger::getLogger()->debug("Move->doOrder(%d): Velocity is [%lld, %lld, %lld] (will arrive at [%lld, %lld, %lld])",
ob->getID(), velo.getX(), velo.getY(), velo.getZ(), arriveat.getX(), arriveat.getY(), arriveat.getZ());
fleet->setVelocity(velo);
uint32_t parentid = ob->getParent();
// recontainerise if necessary
uint32_t containertype = Game::getGame()->getObjectManager()->getObject(parentid)->getContainerType();
Game::getGame()->getObjectManager()->doneWithObject(parentid);
if(fleet->getPosition() != arriveat && containertype >= 1){
//removeFromParent();
std::set<uint32_t> oblist = ((MTSecTurn*)(Game::getGame()->getTurnProcess()))->getContainerIds();
for(std::set<uint32_t>::reverse_iterator itcurr = oblist.rbegin(); itcurr != oblist.rend(); ++itcurr){
IGObject* testedobject = Game::getGame()->getObjectManager()->getObject(*itcurr);
Position3dObjectParam * pos = dynamic_cast<Position3dObjectParam*>(testedobject->getParameterByType(obpT_Position_3D));
SizeObjectParam * size = dynamic_cast<SizeObjectParam*>(testedobject->getParameterByType(obpT_Size));
if(pos == NULL || size == NULL){
Game::getGame()->getObjectManager()->doneWithObject(*itcurr);
continue;
}
Vector3d pos1 = pos->getPosition();
uint64_t size1 = size->getSize();
uint64_t diff = arriveat.getDistance(pos1);
if(diff <= fleet->getSize() / 2 + size1 / 2){
Logger::getLogger()->debug("Container object %d", *itcurr);
//if(Game::getGame()->getObject(*itcurr)->getType() <= 2){
//if(*itcurr != id){
if(size1 >= fleet->getSize()){
if(*itcurr != parentid){
ob->removeFromParent();
ob->addToParent(*itcurr);
}
Game::getGame()->getObjectManager()->doneWithObject(*itcurr);
parentid = *itcurr;
break;
}
}
Game::getGame()->getObjectManager()->doneWithObject(*itcurr);
//}
}
if(parentid == 0){
ob->removeFromParent();
ob->addToParent(0);
}
}
fleet->setPosition(arriveat);
return false;
}
}
void Wormhole::packExtraData(OutputFrame::Ptr frame) {
Vector3d end = getEndB();
frame->packInt64(end.getX());
frame->packInt64(end.getY());
frame->packInt64(end.getZ());
}
bool Vector3d::operator == (const Vector3d& vector){
return (this->x == vector.getX() && this->y == vector.getY() && this->z == vector.getZ());
}
bool Vector3d::operator != (const Vector3d& vector){
return (this->x != vector.getX() && this->y != vector.getY() && this->z != vector.getZ());
}
void Vector3d::add( Vector3d v ) {
this->x += v.getX();
this->y += v.getY();
this->z += v.getZ();
}