/*! This is a helper function for grasp force optimization routines. Given a
set of contacts, and a matrix of contact wrenches expressed in *world*
coordinates and relative to object origin (as computed in gfo routines)
this function will convert them to object coordinate system and accumulate
them in the object's external wrench accumulator. This can serve as an
output of the gfo functions and also for rendering purposes, as the
external wrench accumulator can then be rendered.
*/
void
Grasp::accumulateAndDisplayObjectWrenches(std::list<Contact*> *contacts,
const Matrix &objectWrenches)
{
int count = 0;
std::list<Contact*>::iterator it;
for (it=contacts->begin(); it!=contacts->end(); it++, count++) {
Contact *contact = *it;
vec3 force(objectWrenches.elem(6*count+0,0),
objectWrenches.elem(6*count+1,0),
objectWrenches.elem(6*count+2,0));
vec3 torque(objectWrenches.elem(6*count+3,0),
objectWrenches.elem(6*count+4,0),
objectWrenches.elem(6*count+5,0));
if (contact->getBody2()->isDynamic()) {
DynamicBody *object = (DynamicBody*)(contact->getBody2());
//compute force and torque in body reference frame
//and scale them down for now for rendering and output purposes
force = 1.0e-6 * force * object->getTran().inverse();
//torque is also scaled by maxRadius in conversion matrix
torque = 1.0e-6 * torque * object->getTran().inverse();
//accumulate them on object
object->addForce(force);
object->addTorque(torque);
}
}
}
// temporary one-point version
static void CollideWithTerrain(Body *body)
{
if (!body->IsType(Object::DYNAMICBODY)) return;
DynamicBody *dynBody = static_cast<DynamicBody*>(body);
if (!dynBody->IsMoving()) return;
Frame *f = body->GetFrame();
if (!f || !f->GetBody() || f != f->GetBody()->GetFrame()) return;
if (!f->GetBody()->IsType(Object::TERRAINBODY)) return;
TerrainBody *terrain = static_cast<TerrainBody*>(f->GetBody());
const Aabb &aabb = dynBody->GetAabb();
double altitude = body->GetPosition().Length() + aabb.min.y;
if (altitude >= terrain->GetMaxFeatureRadius()) return;
double terrHeight = terrain->GetTerrainHeight(body->GetPosition().Normalized());
if (altitude >= terrHeight) return;
CollisionContact c;
c.pos = body->GetPosition();
c.normal = c.pos.Normalized();
c.depth = terrHeight - altitude;
c.userData1 = static_cast<void*>(body);
c.userData2 = static_cast<void*>(f->GetBody());
hitCallback(&c);
}
int createEntityWithSize(const char* entityResource, LuaPlus::LuaObject luaPosition, LuaPlus::LuaObject luaSize)
{
if(!luaPosition.IsTable())
{
CORE_LOG("LUA", "Invalid position object passed to create_entity.");
return INVALID_ENTITY_ID;
}
if(!luaSize.IsTable())
{
CORE_LOG("LUA", "Invalid size object passed to create_entity.");
return INVALID_ENTITY_ID;
}
sf::Vector2f position = tableToVec2<sf::Vector2f>(luaPosition);
sf::Vector2f size = tableToVec2<sf::Vector2f>(luaSize);
artemis::Entity& entity = WorldLocator::getObject()->createEntity();
// Create entity
if(!EntityFactory::get().loadFromFile(entityResource, entity))
{
CORE_ERROR("Failed to load entity from file: " + std::string(entityResource));
return INVALID_ENTITY_ID;
}
Transform* pTransform = safeGetComponent<Transform>(&entity);
if(!pTransform)
{
entity.addComponent(new Transform(position.x, position.y));
}
else
{
pTransform->position = position;
}
// Get physics comp
DynamicBody* pDynamicBody = safeGetComponent<DynamicBody>(&entity);
if(pDynamicBody)
{
// Set dimensions, will be initialized later
pDynamicBody->setDimensions(size.x, size.y);
}
else
{
entity.addComponent(new DynamicBody(size.x, size.y));
}
// commit entity changes
entity.refresh();
CORE_LOG("LUA", "Created entity from: " + std::string(entityResource));
return entity.getId();
}
void Space::CollideFrame(Frame *f)
{
if (f->m_astroBody && (f->m_astroBody->IsType(Object::TERRAINBODY))) {
// this is pretty retarded
for (BodyIterator i = m_bodies.begin(); i!=m_bodies.end(); ++i) {
if ((*i)->GetFrame() != f) continue;
if (!(*i)->IsType(Object::DYNAMICBODY)) continue;
DynamicBody *dynBody = static_cast<DynamicBody*>(*i);
Aabb aabb;
dynBody->GetAabb(aabb);
const matrix4x4d &trans = dynBody->GetGeom()->GetTransform();
const vector3d aabbCorners[8] = {
vector3d(aabb.min.x, aabb.min.y, aabb.min.z),
vector3d(aabb.min.x, aabb.min.y, aabb.max.z),
vector3d(aabb.min.x, aabb.max.y, aabb.min.z),
vector3d(aabb.min.x, aabb.max.y, aabb.max.z),
vector3d(aabb.max.x, aabb.min.y, aabb.min.z),
vector3d(aabb.max.x, aabb.min.y, aabb.max.z),
vector3d(aabb.max.x, aabb.max.y, aabb.min.z),
vector3d(aabb.max.x, aabb.max.y, aabb.max.z)
};
CollisionContact c;
for (int j=0; j<8; j++) {
const vector3d &s = aabbCorners[j];
vector3d pos = trans * s;
double terrain_height = static_cast<Planet*>(f->m_astroBody)->GetTerrainHeight(pos.Normalized());
double altitude = pos.Length();
double hitDepth = terrain_height - altitude;
if (altitude < terrain_height) {
c.pos = pos;
c.normal = pos.Normalized();
c.depth = hitDepth;
c.userData1 = static_cast<void*>(dynBody);
c.userData2 = static_cast<void*>(f->m_astroBody);
hitCallback(&c);
}
}
}
}
f->GetCollisionSpace()->Collide(&hitCallback);
for (std::list<Frame*>::iterator i = f->m_children.begin(); i != f->m_children.end(); ++i) {
CollideFrame(*i);
}
}
/*! Asks the dynamics engine to compute the velocities of all bodies at
the current time step. These will be used in the next time step when
the bodies are moved by World::moveDynamicBodies.
The bodies are separated into "islands" of bodies connected by contacts
or joints. Two dynamic bodies are connected if they share a contact or
a joint. Then for each island, this calls the iterate dynamics routine
to build and solve the LCP to find the velocities of all the bodies
in the next iteration.
*/
int GraspitDynamics::computeNewVelocities(double timeStep) {
bool allDynamicsComputed;
static std::list<Contact *> contactList;
std::list<Contact *>::iterator cp;
std::vector<DynamicBody *> robotLinks;
std::vector<DynamicBody *> dynIsland;
std::vector<Robot *> islandRobots;
int i, j, numLinks, numDynBodies, lemkeErrCode;
int numBodies = mWorld->getNumBodies();
#ifdef GRASPITDBG
int islandCount = 0;
#endif
do {
// seed the island with one dynamic body
for (i = 0; i < numBodies; i++)
if (mWorld->getBody(i)->isDynamic() &&
!((DynamicBody *)mWorld->getBody(i))->dynamicsComputed()) {
// if this body is a link, add all robots connected to the link
if (mWorld->getBody(i)->inherits("Link")) {
Robot *robot = ((Robot *)((Link *)mWorld->getBody(i))->getOwner())->getBaseRobot();
robot->getAllLinks(dynIsland);
robot->getAllAttachedRobots(islandRobots);
} else {
dynIsland.push_back((DynamicBody *)mWorld->getBody(i));
}
break;
}
numDynBodies = dynIsland.size();
for (i = 0; i < numDynBodies; i++) {
dynIsland[i]->setDynamicsFlag();
}
// add any bodies that contact any body already in the dynamic island
for (i = 0; i < numDynBodies; i++) {
contactList = dynIsland[i]->getContacts();
for (cp = contactList.begin(); cp != contactList.end(); cp++) {
// if the contacting body is dynamic and not already in the list, add it
if ((*cp)->getBody2()->isDynamic() &&
!((DynamicBody *)(*cp)->getBody2())->dynamicsComputed()) {
DynamicBody *contactedBody = (DynamicBody *)(*cp)->getBody2();
// is this body is a link, add all robots connected to the link
if (contactedBody->isA("Link")) {
Robot *robot = ((Robot *)((Link *)contactedBody)->getOwner())->getBaseRobot();
robot->getAllLinks(robotLinks);
robot->getAllAttachedRobots(islandRobots);
numLinks = robotLinks.size();
for (j = 0; j < numLinks; j++)
if (!robotLinks[j]->dynamicsComputed()) {
dynIsland.push_back(robotLinks[j]);
robotLinks[j]->setDynamicsFlag();
numDynBodies++;
}
robotLinks.clear();
} else {
dynIsland.push_back(contactedBody);
contactedBody->setDynamicsFlag();
numDynBodies++;
}
}
}
}
#ifdef GRASPITDBG
int numIslandRobots = islandRobots.size();
std::cout << "Island " << ++islandCount << " Bodies: ";
for (i = 0; i < numDynBodies; i++) {
std::cout << dynIsland[i]->getName().toStdString().c_str() << " ";
}
std::cout << std::endl;
std::cout << "Island Robots" << islandCount << " Robots: ";
for (i = 0; i < numIslandRobots; i++) {
std::cout << islandRobots[i]->getName().toStdString().c_str() << " ";
}
std::cout << std::endl << std::endl;
#endif
for (i = 0; i < numDynBodies; i++) {
dynIsland[i]->markState();
}
DynamicParameters dp;
if (numDynBodies > 0) {
dp.timeStep = timeStep;
dp.useContactEps = true;
dp.gravityMultiplier = 1.0;
lemkeErrCode = iterateDynamics(islandRobots, dynIsland, &dp);
if (lemkeErrCode == 1) { // dynamics could not be solved
std::cerr << "LCP COULD NOT BE SOLVED!" << std::endl << std::endl;
turnOffDynamics();
return -1;
}
}
dynIsland.clear();
islandRobots.clear();
allDynamicsComputed = true;
for (i = 0; i < numBodies; i++)
if (mWorld->getBody(i)->isDynamic() &&
!((DynamicBody *)mWorld->getBody(i))->dynamicsComputed()) {
allDynamicsComputed = false;
break;
}
} while (!allDynamicsComputed);
// clear all the dynamicsComputed flags
for (i = 0; i < numBodies; i++)
if (mWorld->getBody(i)->isDynamic()) {
((DynamicBody *)mWorld->getBody(i))->resetDynamicsFlag();
}
mWorld->emitDynamicStepTaken();
return 0;
}
static void hitCallback(CollisionContact *c)
{
//printf("OUCH! %x (depth %f)\n", SDL_GetTicks(), c->depth);
Object *po1 = static_cast<Object*>(c->userData1);
Object *po2 = static_cast<Object*>(c->userData2);
const bool po1_isDynBody = po1->IsType(Object::DYNAMICBODY);
const bool po2_isDynBody = po2->IsType(Object::DYNAMICBODY);
// collision response
assert(po1_isDynBody || po2_isDynBody);
if (po1_isDynBody && po2_isDynBody) {
DynamicBody *b1 = static_cast<DynamicBody*>(po1);
DynamicBody *b2 = static_cast<DynamicBody*>(po2);
const vector3d linVel1 = b1->GetVelocity();
const vector3d linVel2 = b2->GetVelocity();
const vector3d angVel1 = b1->GetAngVelocity();
const vector3d angVel2 = b2->GetAngVelocity();
const double coeff_rest = 0.5;
// step back
// mover->UndoTimestep();
const double invMass1 = 1.0 / b1->GetMass();
const double invMass2 = 1.0 / b2->GetMass();
const vector3d hitPos1 = c->pos - b1->GetPosition();
const vector3d hitPos2 = c->pos - b2->GetPosition();
const vector3d hitVel1 = linVel1 + angVel1.Cross(hitPos1);
const vector3d hitVel2 = linVel2 + angVel2.Cross(hitPos2);
const double relVel = (hitVel1 - hitVel2).Dot(c->normal);
// moving away so no collision
if (relVel > 0) return;
if (!OnCollision(po1, po2, c, -relVel)) return;
const double invAngInert1 = 1.0 / b1->GetAngularInertia();
const double invAngInert2 = 1.0 / b2->GetAngularInertia();
const double numerator = -(1.0 + coeff_rest) * relVel;
const double term1 = invMass1;
const double term2 = invMass2;
const double term3 = c->normal.Dot((hitPos1.Cross(c->normal)*invAngInert1).Cross(hitPos1));
const double term4 = c->normal.Dot((hitPos2.Cross(c->normal)*invAngInert2).Cross(hitPos2));
const double j = numerator / (term1 + term2 + term3 + term4);
const vector3d force = j * c->normal;
b1->SetVelocity(linVel1 + force*invMass1);
b1->SetAngVelocity(angVel1 + hitPos1.Cross(force)*invAngInert1);
b2->SetVelocity(linVel2 - force*invMass2);
b2->SetAngVelocity(angVel2 - hitPos2.Cross(force)*invAngInert2);
} else {
// one body is static
vector3d hitNormal;
DynamicBody *mover;
if (po1_isDynBody) {
mover = static_cast<DynamicBody*>(po1);
hitNormal = c->normal;
} else {
mover = static_cast<DynamicBody*>(po2);
hitNormal = -c->normal;
}
const double coeff_rest = 0.5;
const vector3d linVel1 = mover->GetVelocity();
const vector3d angVel1 = mover->GetAngVelocity();
// step back
// mover->UndoTimestep();
const double invMass1 = 1.0 / mover->GetMass();
const vector3d hitPos1 = c->pos - mover->GetPosition();
const vector3d hitVel1 = linVel1 + angVel1.Cross(hitPos1);
const double relVel = hitVel1.Dot(c->normal);
// moving away so no collision
if (relVel > 0 && !c->geomFlag) return;
if (!OnCollision(po1, po2, c, -relVel)) return;
const double invAngInert = 1.0 / mover->GetAngularInertia();
const double numerator = -(1.0 + coeff_rest) * relVel;
const double term1 = invMass1;
const double term3 = c->normal.Dot((hitPos1.Cross(c->normal)*invAngInert).Cross(hitPos1));
const double j = numerator / (term1 + term3);
const vector3d force = j * c->normal;
mover->SetVelocity(linVel1 + force*invMass1);
mover->SetAngVelocity(angVel1 + hitPos1.Cross(force)*invAngInert);
}
}
/*!
Returns the correct coefficient of friction for this contact. If either
body is dynamic, and the relative velocity between them is greater than
1.0 mm/sec (should be made a parameter), then it returns the kinetic COF,
otherwise it returns the static COF.
*/
double
Contact::getCof() const
{
DynamicBody *db;
vec3 radius,vel1(vec3::ZERO),vel2(vec3::ZERO),rotvel;
if (body1->isDynamic()) {
db = (DynamicBody *)body1;
radius = db->getTran().rotation() * (loc - db->getCoG());
vel1.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]);
rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]);
vel1 += radius * rotvel;
}
if (body2->isDynamic()) {
db = (DynamicBody *)body2;
radius = db->getTran().rotation() * (mate->loc - db->getCoG());
vel2.set(db->getVelocity()[0],db->getVelocity()[1],db->getVelocity()[2]);
rotvel.set(db->getVelocity()[3],db->getVelocity()[4],db->getVelocity()[5]);
vel2 += radius * rotvel;
}
if ((vel1 - vel2).len() > 1.0) {
DBGP("SLIDING!");
return kcof;
}
else return cof;
}