void MissileWeapon::updateMovement()
{
if (homing_range > 0)
{
P<SpaceObject> target;
if (game_server)
target = game_server->getObjectById(target_id);
else
target = game_client->getObjectById(target_id);
if (target && (target->getPosition() - getPosition()) < homing_range + target->getRadius())
{
target_angle = sf::vector2ToAngle(target->getPosition() - getPosition());
}
}
float angleDiff = sf::angleDifference(getRotation(), target_angle);
if (angleDiff > 1.0)
setAngularVelocity(turnrate);
else if (angleDiff < -1.0)
setAngularVelocity(turnrate * -1.0f);
else
setAngularVelocity(angleDiff * turnrate);
}
void Physics3DTestDemo::shootBox( const cocos2d::Vec3 &des )
{
Physics3DRigidBodyDes rbDes;
Vec3 linearVel = des - _camera->getPosition3D();
linearVel.normalize();
linearVel *= 100.0f;
rbDes.originalTransform.translate(_camera->getPosition3D());
rbDes.mass = 1.f;
rbDes.shape = Physics3DShape::createBox(Vec3(0.5f, 0.5f, 0.5f));
auto sprite = PhysicsSprite3D::create("Sprite3DTest/box.c3t", &rbDes);
sprite->setTexture("Images/Icon.png");
auto rigidBody = static_cast<Physics3DRigidBody*>(sprite->getPhysicsObj());
rigidBody->setLinearFactor(Vec3::ONE);
rigidBody->setLinearVelocity(linearVel);
rigidBody->setAngularVelocity(Vec3::ZERO);
rigidBody->setCcdMotionThreshold(0.5f);
rigidBody->setCcdSweptSphereRadius(0.4f);
this->addChild(sprite);
sprite->setPosition3D(_camera->getPosition3D());
sprite->setScale(0.5f);
sprite->syncNodeToPhysics();
//optimize, only sync node to physics
sprite->setSyncFlag(Physics3DComponent::PhysicsSyncFlag::PHYSICS_TO_NODE); //sync node to physics
sprite->setCameraMask((unsigned short)CameraFlag::USER1);
}
//Throwing Object
void HelloWorld::shootBoxfunc(const cocos2d::Vec3 &des)
{
//Vec3 linearVel = des -camera->getPosition3D();
//linearVel.normalize();
//linearVel *= 100.0f;
Physics3DRigidBodyDes rbDes;
rbDes.originalTransform.translate(car_cabine->getPosition3D());
rbDes.mass = 1.f;
rbDes.shape = Physics3DShape::createSphere(0.25);
auto sprite = PhysicsSprite3D::create("box.c3t", &rbDes);
sprite->setTexture("Gun.png");
auto rigidBody = static_cast<Physics3DRigidBody*>(sprite->getPhysicsObj());
rigidBody->setLinearFactor(Vec3::ONE);
rigidBody->setLinearVelocity(des*15);
rigidBody->setAngularVelocity(Vec3::ZERO);
rigidBody->setCcdMotionThreshold(0.5f);
rigidBody->setCcdSweptSphereRadius(0.4f);
sprite->setPosition3D(Vec3(car_cabine->getPosition3D().x, car_cabine->getPosition3D().y+4, car_cabine->getPosition3D().z));
sprite->setScale(0.5f);
sprite->syncNodeToPhysics();
sprite->setSyncFlag(Physics3DComponent::PhysicsSyncFlag::PHYSICS_TO_NODE);
sprite->setCameraMask((unsigned short)CameraFlag::USER2);
this->addChild(sprite,1);
}
void PhysicObject::createPhysObject() {
btDefaultMotionState* fallMotionState =
new btDefaultMotionState(btTransform(btQuaternion((initialOrientation.y*GRAD_TO_RAD_COEF), (initialOrientation.x*GRAD_TO_RAD_COEF), (initialOrientation.z*GRAD_TO_RAD_COEF)),btVector3(initialPosition.x, initialPosition.y, initialPosition.z)));
btVector3 fallInertia(0,0,0);
collisionShape->getCollisionShape()->calculateLocalInertia(mass, fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass, fallMotionState, collisionShape->getCollisionShape(), fallInertia);
rigidBody = new btRigidBody(fallRigidBodyCI);
setAngularFactor(angularFactor);
setLinearFactor(linearFactor);
setLinearVelocity(linearVelocity);
rigidBody->setFriction(friction);
rigidBody->setRestitution(restitution);
setAngularVelocity(angularVelocity);
rigidBody->setDamping(linearDumping, angularDumping);
if (_isTrigger) {
rigidBody->setCollisionFlags(rigidBody->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
} else
if (_isKinematic) {
rigidBody->setCollisionFlags(rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
}
rigidBody->setCollisionFlags(rigidBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
rigidBody->setUserPointer(this);
if (!isEnableDeactivation())
rigidBody->setActivationState(DISABLE_DEACTIVATION);
Game::instance->dynamicsWorld->addRigidBody(rigidBody);
}
void ObjectAction::setAngularVelocity(glm::vec3 angularVelocity) {
auto rigidBody = getRigidBody();
if (!rigidBody) {
return;
}
rigidBody->setAngularVelocity(glmToBullet(angularVelocity));
rigidBody->activate();
}
void EntityMotionState::updateObjectVelocities() {
if (_body) {
_sentVelocity = _entity->getVelocityInMeters();
setVelocity(_sentVelocity);
// DANGER! EntityItem stores angularVelocity in degrees/sec!!!
_sentAngularVelocity = glm::radians(_entity->getAngularVelocity());
setAngularVelocity(_sentAngularVelocity);
_sentAcceleration = _entity->getGravityInMeters();
setGravity(_sentAcceleration);
_body->setActivationState(ACTIVE_TAG);
}
}
void Unit::replaceWith(Unit* unit)
{
// Move node
//unit->setPosition(_rootNode->getPosition());
//unit->_rootNode->setRotation(_rootNode->getRotation());
// Copy body state
auto b = _rootNode->getPhysicsBody();
auto ub = unit->_rootNode->getPhysicsBody();
auto pos = b->getPosition();
ub->setPosition(pos.x, pos.y);
//ub->setRotation(b->getRotation());
ub->setVelocity(b->getVelocity());
ub->setAngularVelocity(b->getAngularVelocity());
// Should not we reuse Id?
die();
}
void btRigidBody::internalWritebackVelocity(btScalar timeStep)
{
(void) timeStep;
if (m_inverseMass)
{
setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
setWorldTransform(newTransform);
//m_originalBody->setCompanionId(-1);
}
// m_deltaLinearVelocity.setZero();
// m_deltaAngularVelocity .setZero();
// m_pushVelocity.setZero();
// m_turnVelocity.setZero();
}
void PhysicsSystem::HandleEvents(const frame_tp& timepoint) {
// Remove access to old updated transforms
TransformMap::GetAsyncUpdatedTransforms().Unpublish(timepoint);
// Remove access to forces
async_forces.Unpublish(timepoint);
// Remove access to torques
async_torques.Unpublish(timepoint);
// publish the forces of the current frame immediately without making a copy of the list
async_forces.Publish(std::make_shared<const std::map<id_t,btVector3>>(this->forces.Poll()));
// publish the torques of the current frame
async_torques.Publish(std::make_shared<const std::map<id_t,btVector3>>(this->torques.Poll()));
static frame_tp last_tp;
this->delta = timepoint - last_tp;
last_tp = timepoint;
// Set the rigid bodies linear velocity. Must be done each frame otherwise,
// other forces will stop the linear velocity.
// We use the published list
for (auto& force : *async_forces.GetFuture(timepoint).get()) {
auto body = this->bodies[force.first]->GetRigidBody();
body->setLinearVelocity(force.second + body->getGravity());
}
// Set the rigid bodies angular velocity. Must be done each frame otherwise,
// other forces will stop the angular velocity.
for (auto& torque : *async_torques.GetFuture(timepoint).get()) {
auto body = this->bodies[torque.first]->GetRigidBody();
body->setAngularVelocity(torque.second);
}
if (this->dynamicsWorld) {
dynamicsWorld->stepSimulation(delta.count() * 1.0E-9, 10);
}
// Set out transform updates.
for (auto& shape : this->bodies) {
shape.second->UpdateTransform();
}
// Publish the new updated transforms map
auto ntm = std::make_shared<std::map<id_t,const Transform*>>(TransformMap::GetUpdatedTransforms().Poll());
TransformMap::GetAsyncUpdatedTransforms().Publish(std::move(ntm));
}
void RigidBody::_copyFrom(const ComponentBase *model)
{
init();
auto m = (RigidBody*)model;
#ifdef EDITOR_ENABLED
editorUpdate();
if (editorStruct)
{
editorStruct->copyDatas(m);
editorStruct->refreshDatas(this);
Link *link = entity.getLinkPtr();
if (isKinematic())
{
auto p = posFromMat4(link->getGlobalTransform());
setPosition(posFromMat4(link->getGlobalTransform()));
setRotation(link->getOrientation());
// global orientation not supported
}
}
#else
setAngularDrag(m->getAngularDrag());
setAngularVelocity(m->getAngularVelocity());
setCenterOfMass(m->getCenterOfMass());
setLinearDrag(m->getLinearDrag());
setLinearVelocity(m->getLinearVelocity());
setMass(m->getMass());
setDiagonalInertiaTensor(m->getDiagonalInertiaTensor());
setMaxAngularVelocity(m->getMaxAngularVelocity());
setMaxDepenetrationVelocity(m->getMaxDepenetrationVelocity());
affectByGravity(m->isAffectedByGravity());
setPosition(m->getPosition());
setRotation(m->getRotation());
setAsKinematic(m->isKinematic());
setCollisionDetectionMode(m->getCollisionDetectionMode());
#endif
}
bool PhysicsObject::init(unsigned int attributeIndex, short collisionFilterGroup)
{
if(attributeIndex < 0)
{
return false;
}
attributeIndex_ = attributeIndex;
collisionFilterGroup_ = collisionFilterGroup;
//Get the init data from a physics attribute
AttributePtr<Attribute_Physics> ptr_physics = itrPhysics_.at(attributeIndex);
btScalar mass = static_cast<btScalar>(ptr_physics->mass);
//Resolve mass, local inertia of the collision shape, and also the collision shape itself.
btCollisionShape* collisionShape = subClassSpecificCollisionShape();
if(collisionShape != nullptr)
{
setCollisionShape(collisionShape);
}
else
{
ERROR_MESSAGEBOX("Error in PhysicsObject::init. Expected collision shape pointer unexpectedly set to nullptr. Using default shape instead.");
setCollisionShape(CollisionShapes::Instance()->getDefaultShape());
}
btVector3 localInertia = subClassCalculateLocalInertiaHook(mass);
setMassProps(mass, localInertia); //Set inverse mass and inverse local inertia
updateInertiaTensor();
if((getCollisionFlags() & btCollisionObject::CF_STATIC_OBJECT))
{
btTransform world;
AttributePtr<Attribute_Spatial> ptr_spatial = itrPhysics_.at(attributeIndex_)->ptr_spatial;
AttributePtr<Attribute_Position> ptr_position = ptr_spatial->ptr_position;
world.setOrigin(convert(ptr_position->position));
world.setRotation(convert(ptr_spatial->rotation));
setWorldTransform(world); //Static physics objects: transform once
}
else
{
//Non-static physics objects: let a derived btMotionState handle transforms.
MotionState* customMotionState = new MotionState(attributeIndex);
setMotionState(customMotionState);
setAngularVelocity(btVector3(ptr_physics->angularVelocity.x,
ptr_physics->angularVelocity.y,
ptr_physics->angularVelocity.z));
setLinearVelocity(btVector3(ptr_physics->linearVelocity.x,
ptr_physics->linearVelocity.y,
ptr_physics->linearVelocity.z));
//Gravity is set after "addRigidBody" for non-static physics objects
}
if(ptr_physics->collisionResponse)
{
setCollisionFlags(getCollisionFlags() & ~CF_NO_CONTACT_RESPONSE); //Activate collision response
}
else
{
setCollisionFlags(getCollisionFlags() | CF_NO_CONTACT_RESPONSE); //Deactivate collision response
}
return subClassSpecificInitHook();
}
void ClientUnit::simulateUnit()
{
// Apply latest frame inputs just before simulation
Real time = TIME_PER_FRAME;
mScale = mSpeed*time;
mAgressorTime += time;
mTargetTime += time;
mFireTime += time;
mMoveTime += time;
if (mAgressorTime >= TIME_TO_PEACE) {
mAgressor = 0;
}
if (mTargetTime >= TIME_TO_PEACE) {
mTarget = 0;
}
// Motion control
mInput->vMove.x = ((int)mInput->aKey[KEY_LEFT ] - (int)mInput->aKey[KEY_RIGHT ]);
mInput->vMove.y = ((int)mInput->aKey[KEY_UP ] - (int)mInput->aKey[KEY_DOWN ]);
mInput->vMove.z = ((int)mInput->aKey[KEY_FORWARD] - (int)mInput->aKey[KEY_BACKWARD]);
mInput->vRotate.x = -(int)mInput->aAxe[0];
mInput->vRotate.y = (int)mInput->aAxe[1];
// Apply motion control
if (mCtrl != MODE_GHOST) {
if (!maState[0] && mInput->aKey[KEY_FORWARD] || !maState[1] && mInput->aKey[KEY_BACKWARD]) {
this->setLinearVelocity(Vec3(0.0f, 0.0f, 0.0f));
this->setAngularVelocity(Vec3(0.0f, 0.0f, 0.0f));
this->setLinearMomentum(Vec3(0.0f, 0.0f, 0.0f));
this->setAngularMomentum(Vec3(0.0f, 0.0f, 0.0f));
}
if (mInput->aKey[KEY_BREAK]) {
this->setLinearVelocity(Vec3(0.0f, 0.0f, 0.0f));
this->setAngularVelocity(Vec3(0.0f, 0.0f, 0.0f));
this->setLinearMomentum(Vec3(0.0f, 0.0f, 0.0f));
this->setAngularMomentum(Vec3(0.0f, 0.0f, 0.0f));
}
if (!maState[3] && mInput->aKey[KEY_LEFT] || !maState[4] && mInput->aKey[KEY_RIGHT]) {
if (mSaladin->maReferentiel[0]->speed > 10.0f) {
setAngularVelocity(Vec3(0.0f, 0.0f, 0.0f));
setAngularMomentum(Vec3(0.0f, 0.0f, 0.0f));
}
}
if (!maState[2]) {
mMoveTime = 0;
}
mSaladin->simulateRotation(
mInput->vRotate.x*mScale.y,
mInput->vRotate.y*mScale.y
);
mSaladin->simulateTorque(
mInput->aKey[KEY_BREAK], // DIRBRAKE
mInput->vMove.z, // DIRTORQUE
mInput->vMove.x, // DIRSTEER
mSpeed, // speed
mScale, // scaled speed
mDiff // TORQUE DIFFERENTIAL
);
simulateFire(
mInput->aButton[0]
);
if (mInput->aKey[KEY_FORWARD]) {
maState[0] = true;
maState[1] = false;
maState[2] = false;
}
if (mInput->aKey[KEY_BACKWARD]) {
maState[0] = false;
maState[1] = true;
maState[2] = false;
}
if (mInput->aKey[KEY_LEFT] || mInput->aKey[KEY_RIGHT]) {
maState[2] = false;
}
maState[3] = mInput->aKey[KEY_LEFT];
maState[4] = mInput->aKey[KEY_RIGHT];
if (mInput->aKey[KEY_BREAK]) {
maState[0] = true;
maState[1] = false;
maState[2] = true;
maState[3] = false;
maState[4] = false;
}
}
}
U32 LLVOTree::processUpdateMessage(LLMessageSystem *mesgsys,
void **user_data,
U32 block_num, EObjectUpdateType update_type,
LLDataPacker *dp)
{
// Do base class updates...
U32 retval = LLViewerObject::processUpdateMessage(mesgsys, user_data, block_num, update_type, dp);
if ( (getVelocity().lengthSquared() > 0.f)
||(getAcceleration().lengthSquared() > 0.f)
||(getAngularVelocity().lengthSquared() > 0.f))
{
llinfos << "ACK! Moving tree!" << llendl;
setVelocity(LLVector3::zero);
setAcceleration(LLVector3::zero);
setAngularVelocity(LLVector3::zero);
}
if (update_type == OUT_TERSE_IMPROVED)
{
// Nothing else needs to be done for the terse message.
return retval;
}
//
// Load Instance-Specific data
//
if (mData)
{
mSpecies = ((U8 *)mData)[0];
}
if (!sSpeciesTable.count(mSpecies))
{
if (sSpeciesTable.size())
{
SpeciesMap::const_iterator it = sSpeciesTable.begin();
mSpecies = (*it).first;
}
}
//
// Load Species-Specific data
//
mTreeImagep = gImageList.getImage(sSpeciesTable[mSpecies]->mTextureID);
if (mTreeImagep)
{
gGL.getTexUnit(0)->bind(mTreeImagep.get());
}
mBranchLength = sSpeciesTable[mSpecies]->mBranchLength;
mTrunkLength = sSpeciesTable[mSpecies]->mTrunkLength;
mLeafScale = sSpeciesTable[mSpecies]->mLeafScale;
mDroop = sSpeciesTable[mSpecies]->mDroop;
mTwist = sSpeciesTable[mSpecies]->mTwist;
mBranches = sSpeciesTable[mSpecies]->mBranches;
mDepth = sSpeciesTable[mSpecies]->mDepth;
mScaleStep = sSpeciesTable[mSpecies]->mScaleStep;
mTrunkDepth = sSpeciesTable[mSpecies]->mTrunkDepth;
mBillboardScale = sSpeciesTable[mSpecies]->mBillboardScale;
mBillboardRatio = sSpeciesTable[mSpecies]->mBillboardRatio;
mTrunkAspect = sSpeciesTable[mSpecies]->mTrunkAspect;
mBranchAspect = sSpeciesTable[mSpecies]->mBranchAspect;
// position change not caused by us, etc. make sure to rebuild.
gPipeline.markRebuild(mDrawable, LLDrawable::REBUILD_ALL);
return retval;
}
void EntityItem::update(const quint64& updateTime) {
bool wantDebug = false;
if (_lastUpdated == 0) {
_lastUpdated = updateTime;
}
float timeElapsed = (float)(updateTime - _lastUpdated) / (float)(USECS_PER_SECOND);
if (wantDebug) {
qDebug() << "********** EntityItem::update()";
qDebug() << " entity ID=" << getEntityItemID();
qDebug() << " updateTime=" << updateTime;
qDebug() << " _lastUpdated=" << _lastUpdated;
qDebug() << " timeElapsed=" << timeElapsed;
qDebug() << " hasVelocity=" << hasVelocity();
qDebug() << " hasGravity=" << hasGravity();
qDebug() << " isRestingOnSurface=" << isRestingOnSurface();
qDebug() << " hasAngularVelocity=" << hasAngularVelocity();
qDebug() << " getAngularVelocity=" << getAngularVelocity();
qDebug() << " isMortal=" << isMortal();
qDebug() << " getAge()=" << getAge();
qDebug() << " getLifetime()=" << getLifetime();
if (hasVelocity() || (hasGravity() && !isRestingOnSurface())) {
qDebug() << " MOVING...=";
qDebug() << " hasVelocity=" << hasVelocity();
qDebug() << " hasGravity=" << hasGravity();
qDebug() << " isRestingOnSurface=" << isRestingOnSurface();
qDebug() << " hasAngularVelocity=" << hasAngularVelocity();
qDebug() << " getAngularVelocity=" << getAngularVelocity();
}
if (hasAngularVelocity()) {
qDebug() << " CHANGING...=";
qDebug() << " hasAngularVelocity=" << hasAngularVelocity();
qDebug() << " getAngularVelocity=" << getAngularVelocity();
}
if (isMortal()) {
qDebug() << " MORTAL...=";
qDebug() << " isMortal=" << isMortal();
qDebug() << " getAge()=" << getAge();
qDebug() << " getLifetime()=" << getLifetime();
}
}
_lastUpdated = updateTime;
if (wantDebug) {
qDebug() << " ********** EntityItem::update() .... SETTING _lastUpdated=" << _lastUpdated;
}
if (hasAngularVelocity()) {
glm::quat rotation = getRotation();
glm::vec3 angularVelocity = glm::radians(getAngularVelocity());
float angularSpeed = glm::length(angularVelocity);
if (angularSpeed < EPSILON_VELOCITY_LENGTH) {
setAngularVelocity(NO_ANGULAR_VELOCITY);
} else {
float angle = timeElapsed * angularSpeed;
glm::quat dQ = glm::angleAxis(angle, glm::normalize(angularVelocity));
rotation = dQ * rotation;
setRotation(rotation);
// handle damping for angular velocity
if (getAngularDamping() > 0.0f) {
glm::vec3 dampingResistance = getAngularVelocity() * getAngularDamping();
glm::vec3 newAngularVelocity = getAngularVelocity() - (dampingResistance * timeElapsed);
setAngularVelocity(newAngularVelocity);
if (wantDebug) {
qDebug() << " getDamping():" << getDamping();
qDebug() << " dampingResistance:" << dampingResistance;
qDebug() << " newAngularVelocity:" << newAngularVelocity;
}
}
}
}
if (hasVelocity() || hasGravity()) {
glm::vec3 position = getPosition();
glm::vec3 velocity = getVelocity();
glm::vec3 newPosition = position + (velocity * timeElapsed);
if (wantDebug) {
qDebug() << " EntityItem::update()....";
qDebug() << " timeElapsed:" << timeElapsed;
qDebug() << " old AACube:" << getMaximumAACube();
qDebug() << " old position:" << position;
qDebug() << " old velocity:" << velocity;
qDebug() << " old getAABox:" << getAABox();
qDebug() << " getDistanceToBottomOfEntity():" << getDistanceToBottomOfEntity() * (float)TREE_SCALE << " in meters";
qDebug() << " newPosition:" << newPosition;
qDebug() << " glm::distance(newPosition, position):" << glm::distance(newPosition, position);
}
position = newPosition;
// handle bounces off the ground... We bounce at the distance to the bottom of our entity
if (position.y <= getDistanceToBottomOfEntity()) {
velocity = velocity * glm::vec3(1,-1,1);
// if we've slowed considerably, then just stop moving
if (glm::length(velocity) <= EPSILON_VELOCITY_LENGTH) {
velocity = NO_VELOCITY;
}
position.y = getDistanceToBottomOfEntity();
}
// handle gravity....
if (hasGravity() && !isRestingOnSurface()) {
velocity += getGravity() * timeElapsed;
}
// handle resting on surface case, this is definitely a bit of a hack, and it only works on the
// "ground" plane of the domain, but for now it
if (hasGravity() && isRestingOnSurface()) {
velocity.y = 0.0f;
position.y = getDistanceToBottomOfEntity();
}
// handle damping for velocity
glm::vec3 dampingResistance = velocity * getDamping();
if (wantDebug) {
qDebug() << " getDamping():" << getDamping();
qDebug() << " dampingResistance:" << dampingResistance;
qDebug() << " dampingResistance * timeElapsed:" << dampingResistance * timeElapsed;
}
velocity -= dampingResistance * timeElapsed;
if (wantDebug) {
qDebug() << " velocity AFTER dampingResistance:" << velocity;
qDebug() << " glm::length(velocity):" << glm::length(velocity);
qDebug() << " EPSILON_VELOCITY_LENGTH:" << EPSILON_VELOCITY_LENGTH;
}
// round velocity to zero if it's close enough...
if (glm::length(velocity) <= EPSILON_VELOCITY_LENGTH) {
velocity = NO_VELOCITY;
}
setPosition(position); // this will automatically recalculate our collision shape
setVelocity(velocity);
if (wantDebug) {
qDebug() << " new position:" << position;
qDebug() << " new velocity:" << velocity;
qDebug() << " new AACube:" << getMaximumAACube();
qDebug() << " old getAABox:" << getAABox();
}
}
}
bool Physics3DConstraintDemo::init()
{
if (!Physics3DTestDemo::init())
return false;
//PhysicsSprite3D = Sprite3D + Physics3DComponent
Physics3DRigidBodyDes rbDes;
rbDes.disableSleep = true;
//create box
auto sprite = Sprite3D::create("Sprite3DTest/orc.c3b");
rbDes.mass = 10.f;
rbDes.shape = Physics3DShape::createBox(Vec3(5.0f, 5.0f, 5.0f));
auto rigidBody = Physics3DRigidBody::create(&rbDes);
Quaternion quat;
Quaternion::createFromAxisAngle(Vec3(0.f, 1.f, 0.f), CC_DEGREES_TO_RADIANS(180), &quat);
auto component = Physics3DComponent::create(rigidBody, Vec3(0.f, -3.f, 0.f), quat);
sprite->addComponent(component);
addChild(sprite);
sprite->setCameraMask((unsigned short)CameraFlag::USER1);
sprite->setScale(0.4f);
sprite->setPosition3D(Vec3(-20.f, 5.f, 0.f));
//sync node position to physics
component->syncNodeToPhysics();
//physics controlled, we will not set position for it, so we can skip sync node position to physics
component->setSyncFlag(Physics3DComponent::PhysicsSyncFlag::PHYSICS_TO_NODE);
physicsScene->setPhysics3DDebugCamera(_camera);
//create point to point constraint
Physics3DConstraint* constraint = Physics3DPointToPointConstraint::create(rigidBody, Vec3(2.5f, 2.5f, 2.5f));
physicsScene->getPhysics3DWorld()->addPhysics3DConstraint(constraint);
//create hinge constraint
rbDes.mass = 1.0f;
rbDes.shape = Physics3DShape::createBox(Vec3(8.0f, 8.0f, 1.f));
rigidBody = Physics3DRigidBody::create(&rbDes);
component = Physics3DComponent::create(rigidBody);
sprite = Sprite3D::create("Sprite3DTest/box.c3t");
sprite->setTexture("Sprite3DTest/plane.png");
sprite->setScaleX(8.f);
sprite->setScaleY(8.f);
sprite->setPosition3D(Vec3(5.f, 0.f, 0.f));
sprite->addComponent(component);
sprite->setCameraMask((unsigned short)CameraFlag::USER1);
this->addChild(sprite);
component->syncNodeToPhysics();
rigidBody->setAngularVelocity(Vec3(0,3,0));
constraint = Physics3DHingeConstraint::create(rigidBody, Vec3(4.f, 4.f, 0.5f), Vec3(0.f, 1.f, 0.f));
physicsScene->getPhysics3DWorld()->addPhysics3DConstraint(constraint);
//create slider constraint
rbDes.mass = 1.0f;
rbDes.shape = Physics3DShape::createBox(Vec3(3.0f, 2.0f, 3.f));
rigidBody = Physics3DRigidBody::create(&rbDes);
component = Physics3DComponent::create(rigidBody);
sprite = Sprite3D::create("Sprite3DTest/box.c3t");
sprite->setTexture("Sprite3DTest/plane.png");
sprite->setScaleX(3.f);
sprite->setScaleZ(3.f);
sprite->setPosition3D(Vec3(30.f, 15.f, 0.f));
sprite->addComponent(component);
sprite->setCameraMask((unsigned short)CameraFlag::USER1);
this->addChild(sprite);
component->syncNodeToPhysics();
rigidBody->setLinearVelocity(Vec3(0,3,0));
rbDes.mass = 0.0f;
rbDes.shape = Physics3DShape::createBox(Vec3(3.0f, 3.0f, 3.f));
auto rigidBodyB = Physics3DRigidBody::create(&rbDes);
component = Physics3DComponent::create(rigidBodyB);
sprite = Sprite3D::create("Sprite3DTest/box.c3t");
sprite->setTexture("Sprite3DTest/plane.png");
sprite->setScale(3.f);
sprite->setPosition3D(Vec3(30.f, 5.f, 0.f));
sprite->addComponent(component);
sprite->setCameraMask((unsigned short)CameraFlag::USER1);
this->addChild(sprite);
component->syncNodeToPhysics();
Mat4 frameInA, frameInB;
Mat4::createRotationZ(CC_DEGREES_TO_RADIANS(90), &frameInA);
frameInB = frameInA;
frameInA.m[13] = -5.f;
frameInB.m[13] = 5.f;
constraint = Physics3DSliderConstraint::create(rigidBody, rigidBodyB, frameInA, frameInB, false);
physicsScene->getPhysics3DWorld()->addPhysics3DConstraint(constraint);
((Physics3DSliderConstraint*)constraint)->setLowerLinLimit(-5.f);
((Physics3DSliderConstraint*)constraint)->setUpperLinLimit(5.f);
//create ConeTwist constraint
rbDes.mass = 1.f;
rbDes.shape = Physics3DShape::createBox(Vec3(3.f, 3.f, 3.f));
rigidBody = Physics3DRigidBody::create(&rbDes);
component = Physics3DComponent::create(rigidBody);
sprite = Sprite3D::create("Sprite3DTest/box.c3t");
sprite->setTexture("Sprite3DTest/plane.png");
sprite->setScale(3.f);
sprite->setPosition3D(Vec3(-10.f, 5.f, 0.f));
sprite->addComponent(component);
sprite->setCameraMask((unsigned short)CameraFlag::USER1);
this->addChild(sprite);
component->syncNodeToPhysics();
Mat4::createRotationZ(CC_DEGREES_TO_RADIANS(90), &frameInA);
frameInA.m[12] = 0.f;
frameInA.m[13] = -10.f;
frameInA.m[14] = 0.f;
constraint = Physics3DConeTwistConstraint::create(rigidBody, frameInA);
physicsScene->getPhysics3DWorld()->addPhysics3DConstraint(constraint, true);
((Physics3DConeTwistConstraint*)constraint)->setLimit(CC_DEGREES_TO_RADIANS(10), CC_DEGREES_TO_RADIANS(10), CC_DEGREES_TO_RADIANS(40));
//create 6 dof constraint
rbDes.mass = 1.0f;
rbDes.shape = Physics3DShape::createBox(Vec3(3.0f, 3.0f, 3.f));
rigidBody = Physics3DRigidBody::create(&rbDes);
component = Physics3DComponent::create(rigidBody);
sprite = Sprite3D::create("Sprite3DTest/box.c3t");
sprite->setTexture("Sprite3DTest/plane.png");
sprite->setScale(3.f);
sprite->setPosition3D(Vec3(30.f, -5.f, 0.f));
sprite->addComponent(component);
sprite->setCameraMask((unsigned short)CameraFlag::USER1);
this->addChild(sprite);
component->syncNodeToPhysics();
frameInA.setIdentity();
constraint = Physics3D6DofConstraint::create(rigidBody, frameInA, false);
physicsScene->getPhysics3DWorld()->addPhysics3DConstraint(constraint);
((Physics3D6DofConstraint*)constraint)->setAngularLowerLimit(Vec3(0,0,0));
((Physics3D6DofConstraint*)constraint)->setAngularUpperLimit(Vec3(0,0,0));
((Physics3D6DofConstraint*)constraint)->setLinearLowerLimit(Vec3(-10,0,0));
((Physics3D6DofConstraint*)constraint)->setLinearUpperLimit(Vec3(10,0,0));
return true;
}
int PhysicObject::methodsBridge(lua_State* luaVM) {
if (isCurrentMethod("applyImpulse")) {
applyImpulse(CVector(lua_tonumber(luaVM, 1), lua_tonumber(luaVM, 2), lua_tonumber(luaVM, 3)), CVector(lua_tonumber(luaVM, 4), lua_tonumber(luaVM, 5), lua_tonumber(luaVM, 6)));
return 0;
}
if (isCurrentMethod("applyForce")) {
applyForce(CVector(lua_tonumber(luaVM, 1), lua_tonumber(luaVM, 2), lua_tonumber(luaVM, 3)), CVector(lua_tonumber(luaVM, 4), lua_tonumber(luaVM, 5), lua_tonumber(luaVM, 6)));
return 0;
}
if (isCurrentMethod("setLinearVelocity")) {
setLinearVelocity(CVector(lua_tonumber(luaVM, 1), lua_tonumber(luaVM, 2), lua_tonumber(luaVM, 3)));
return 0;
}
if (isCurrentMethod("getLinearVelocity")) {
luaPushVector(luaVM, getLinearVelocity().x, getLinearVelocity().y, getLinearVelocity().z);
return 1;
}
if (isCurrentMethod("setMass")) {
setMass(lua_tonumber(luaVM, 1));
return 0;
}
if (isCurrentMethod("getMass")) {
lua_pushnumber(luaVM, getMass());
return 1;
}
if (isCurrentMethod("setCollisionShapeType")) {
setCollisionShapeType((CollisionShapeType)lua_tointeger(luaVM, 1));
return 0;
}
if (isCurrentMethod("getCollisionShapeType")) {
lua_pushinteger(luaVM, getCollisionShapeType());
return 1;
}
if (isCurrentMethod("enablePhysics")) {
enablePhysics(lua_toboolean(luaVM, 1));
return 0;
}
if (isCurrentMethod("isEnabledPhysics")) {
lua_pushboolean(luaVM, isEnabledPhysics());
return 1;
}
if (isCurrentMethod("setAngularFactor")) {
setAngularFactor(CVector(lua_tonumber(luaVM, 1), lua_tonumber(luaVM, 2), lua_tonumber(luaVM, 3)));
return 0;
}
if (isCurrentMethod("getAngularFactor")) {
luaPushVector(luaVM, getAngularFactor().x, getAngularFactor().y, getAngularFactor().z);
return 1;
}
if (isCurrentMethod("setLinearFactor")) {
setLinearFactor(CVector(lua_tonumber(luaVM, 1), lua_tonumber(luaVM, 2), lua_tonumber(luaVM, 3)));
return 0;
}
if (isCurrentMethod("getLinearFactor")) {
luaPushVector(luaVM, getLinearFactor().x, getLinearFactor().y, getLinearFactor().z);
return 1;
}
if (isCurrentMethod("setTrigger")) {
setTrigger(lua_toboolean(luaVM, 1));
return 0;
}
if (isCurrentMethod("isTrigger")) {
lua_pushboolean(luaVM, isTrigger());
return 1;
}
if (isCurrentMethod("getCollisionShape")) {
if (getCollisionShape() == NULL) {
lua_pushnil(luaVM);
} else {
lua_getglobal(luaVM, getCollisionShape()->getGOID().c_str());
}
return 1;
}
if (isCurrentMethod("setCollisionShape")) {
if (lua_isnil(luaVM, 1)) {
setCollisionShape(NULL);
return 0;
}
lua_pushstring(luaVM, "cpointer");
lua_gettable(luaVM, -2);
GOCollisionShape* o = (GOCollisionShape*)lua_tointeger(luaVM, -1);
setCollisionShape(o);
lua_pop(luaVM, -1);
return 0;
}
if (isCurrentMethod("setEnableDeactivation")) {
setEnableDeactivation(lua_toboolean(luaVM, 1));
return 0;
}
if (isCurrentMethod("isEnableDeactivation")) {
lua_pushboolean(luaVM, isEnableDeactivation());
return 1;
}
if (isCurrentMethod("setFriction")) {
setFriction(lua_tonumber(luaVM, 1));
return 0;
}
if (isCurrentMethod("getFriction")) {
lua_pushnumber(luaVM, getFriction());
return 1;
}
if (isCurrentMethod("setRestitution")) {
setRestitution(lua_tonumber(luaVM, 1));
return 0;
}
if (isCurrentMethod("getRestitution")) {
lua_pushnumber(luaVM, getRestitution());
return 1;
}
if (isCurrentMethod("setLinearDumping")) {
setLinearDumping(lua_tonumber(luaVM, 1));
return 0;
}
if (isCurrentMethod("getLinearDumping")) {
lua_pushnumber(luaVM, getLinearDumping());
return 1;
}
if (isCurrentMethod("setAngularDumping")) {
setAngularDumping(lua_tonumber(luaVM, 1));
return 0;
}
if (isCurrentMethod("getAngularDumping")) {
lua_pushnumber(luaVM, getAngularDumping());
return 1;
}
if (isCurrentMethod("setAngularVelocity")) {
setAngularVelocity(CVector(lua_tonumber(luaVM, 1), lua_tonumber(luaVM, 2), lua_tonumber(luaVM, 3)));
return 0;
}
if (isCurrentMethod("getAngularVelocity")) {
CVector av = getAngularVelocity();
luaPushVector(luaVM, av.x, av.y, av.z);
return 1;
}
if (isCurrentMethod("addConstraint")) {
if (lua_isnil(luaVM, 1)) {
return 0;
}
lua_pushstring(luaVM, "cpointer");
lua_gettable(luaVM, -2);
GOConstraint* o = (GOConstraint*)lua_tointeger(luaVM, -1);
addConstraint(o);
lua_pop(luaVM, -1);
return 0;
}
if (isCurrentMethod("getConstraints")) {
lua_newtable(luaVM);
int tableIndex = lua_gettop(luaVM);
vector<GOConstraint*> objs;
for (unsigned int i = 0; i < constraints.size(); ++i) {
if (constraints.at(i)->id == "undefined" || constraints.at(i)->id == "") continue;
objs.push_back(constraints.at(i));
}
for (unsigned int i = 0; i < objs.size(); ++i) {
lua_pushinteger(luaVM, i+1);
lua_getglobal(luaVM, objs.at(i)->id.c_str());
lua_settable (luaVM, tableIndex);
}
return 1;
}
if (isCurrentMethod("removeConstraint")) {
if (lua_isnil(luaVM, 1)) {
return 0;
}
lua_pushstring(luaVM, "cpointer");
lua_gettable(luaVM, -2);
GOConstraint* o = (GOConstraint*)lua_tointeger(luaVM, -1);
removeConstraint(o);
lua_pop(luaVM, -1);
return 0;
}
if (isCurrentMethod("removeAllConstrains")) {
removeAllConstraints();
return 0;
}
if (isCurrentMethod("secondObjectForConstraint")) {
if (lua_isnil(luaVM, 1)) {
return 0;
}
lua_pushstring(luaVM, "cpointer");
lua_gettable(luaVM, -2);
GOConstraint* o = (GOConstraint*)lua_tointeger(luaVM, -1);
secondObjectForConstraint(o);
lua_pop(luaVM, -1);
return 0;
}
if (isCurrentMethod("isSecondObjectForConstraints")) {
lua_newtable(luaVM);
int tableIndex = lua_gettop(luaVM);
vector<GOConstraint*> objs;
for (unsigned int i = 0; i < secondObjectForConstraints.size(); ++i) {
if (secondObjectForConstraints.at(i)->id == "undefined" || secondObjectForConstraints.at(i)->id == "") continue;
objs.push_back(secondObjectForConstraints.at(i));
}
for (unsigned int i = 0; i < objs.size(); ++i) {
lua_pushinteger(luaVM, i+1);
lua_getglobal(luaVM, objs.at(i)->id.c_str());
lua_settable (luaVM, tableIndex);
}
return 1;
}
if (isCurrentMethod("notUseAsSecondObjectForConstraint")) {
if (lua_isnil(luaVM, 1)) {
return 0;
}
lua_pushstring(luaVM, "cpointer");
lua_gettable(luaVM, -2);
GOConstraint* o = (GOConstraint*)lua_tointeger(luaVM, -1);
notUseAsSecondObjectForConstraint(o);
lua_pop(luaVM, -1);
return 0;
}
return LuaBridge::methodsBridge(luaVM);
}
U32 LLVOTree::processUpdateMessage(LLMessageSystem *mesgsys,
void **user_data,
U32 block_num, EObjectUpdateType update_type,
LLDataPacker *dp)
{
// Do base class updates...
U32 retval = LLViewerObject::processUpdateMessage(mesgsys, user_data, block_num, update_type, dp);
if ( (getVelocity().lengthSquared() > 0.f)
||(getAcceleration().lengthSquared() > 0.f)
||(getAngularVelocity().lengthSquared() > 0.f))
{
llinfos << "ACK! Moving tree!" << llendl;
setVelocity(LLVector3::zero);
setAcceleration(LLVector3::zero);
setAngularVelocity(LLVector3::zero);
}
if (update_type == OUT_TERSE_IMPROVED)
{
// Nothing else needs to be done for the terse message.
return retval;
}
//
// Load Instance-Specific data
//
if (mData)
{
mSpecies = ((U8 *)mData)[0];
}
if (!sSpeciesTable.count(mSpecies))
{
if (sSpeciesTable.size())
{
SpeciesMap::const_iterator it = sSpeciesTable.begin();
mSpecies = (*it).first;
}
}
//
// Load Species-Specific data
//
static const S32 MAX_TREE_TEXTURE_VIRTURE_SIZE_RESET_INTERVAL = 32 ; //frames.
mTreeImagep = LLViewerTextureManager::getFetchedTexture(sSpeciesTable[mSpecies]->mTextureID, TRUE, LLViewerTexture::BOOST_NONE, LLViewerTexture::LOD_TEXTURE);
mTreeImagep->setMaxVirtualSizeResetInterval(MAX_TREE_TEXTURE_VIRTURE_SIZE_RESET_INTERVAL); //allow to wait for at most 16 frames to reset virtual size.
mBranchLength = sSpeciesTable[mSpecies]->mBranchLength;
mTrunkLength = sSpeciesTable[mSpecies]->mTrunkLength;
mLeafScale = sSpeciesTable[mSpecies]->mLeafScale;
mDroop = sSpeciesTable[mSpecies]->mDroop;
mTwist = sSpeciesTable[mSpecies]->mTwist;
mBranches = sSpeciesTable[mSpecies]->mBranches;
mDepth = sSpeciesTable[mSpecies]->mDepth;
mScaleStep = sSpeciesTable[mSpecies]->mScaleStep;
mTrunkDepth = sSpeciesTable[mSpecies]->mTrunkDepth;
mBillboardScale = sSpeciesTable[mSpecies]->mBillboardScale;
mBillboardRatio = sSpeciesTable[mSpecies]->mBillboardRatio;
mTrunkAspect = sSpeciesTable[mSpecies]->mTrunkAspect;
mBranchAspect = sSpeciesTable[mSpecies]->mBranchAspect;
// position change not caused by us, etc. make sure to rebuild.
gPipeline.markRebuild(mDrawable, LLDrawable::REBUILD_ALL);
return retval;
}
int main() {
/*system initialization */
e_init_port();
/* Init UART1 for bluetooth */
e_init_uart1();
/* Init UART2 for e-randb */
e_init_uart2();
/* Init IR */
e_init_prox();
//init motors
e_init_motors();
/* Wait for a command comming from bluetooth IRCOMTEST on pc directory*/
btcomWaitForCommand('s');
/* Start agendas processing which will take care of UART interruptions */
e_start_agendas_processing();
/* Init E-RANDB board */
e_init_randb();
/* Range is tunable by software.
* 0 -> Full Range (1m. approx depending on light conditions )
* 255 --> No Range (0cm. approx, depending on light conditions */
e_randb_uart_set_range(0);
/* At some point we tought that the board could just take
* data en leave the calculations for the robot.
* At the moment, it is better to allow the board to do the calculations */
e_randb_uart_set_calculation(ON_BOARD);
/* Store light conditions to use them as offset for the calculation
* of the range and bearing */
e_randb_uart_store_light_conditions();
e_randb_set_uart_communication(1);
finalDataRegister data;
//tabla comunciacion
double comunicacionAngulos[2];
int comunicacionRangos[2];
//subsuncion
int CURRENT_STATE;
int subsuncion[2][2];
int debug_var = 0;
subsuncion[0][0]=SPACING;
subsuncion[1][0]=COHESION;
int i;
for (i=0;i<2;i++){
subsuncion[i][1]=0;
}
//proximity sensors reading
int prox_first_reading[8];
int prox_reading[8];
/* Angles in rad for IRs starting at 0. Left direction. */
const double prox_directions[8] = {5.9865, 5.4105, 4.7124, 3.6652, 2.6180, 1.5708, 0.8727, 0.2967};
/* Get the first reading to take ambient light */
for(i=0; i < 8; i++){
prox_first_reading[i]=e_get_prox(i);
}
/* Print on the bluetooth */
char tmp2[50];
sprintf(tmp2,"-- CHASER --\n");
btcomSendString(tmp2);
while(1) {
//comprobacion proximidad
int maxProx = 0;
/* Get readings and substract the first reading */
for(i=0; i < 8; i++){
prox_reading[i] = e_get_prox(i) - prox_first_reading[i];
if(prox_reading[i] < 0) {prox_reading[i] = 0; }
if ( prox_reading[i]>maxProx){
maxProx = prox_reading[i];
}
}
if(maxProx > PROX_THRES){
subsuncion[0][1]=1; // Collission
}
else{ // Chasing
subsuncion[0][1]=0;
}
CURRENT_STATE = 1; //by default. chasing
for(i=0;i<2;i++){
if(subsuncion[i][1]==1){
CURRENT_STATE = i;
break;
}
}
char tmp[30];
double vector_repelent[2] = {0.0,0.0};
double ang_repelent;
double ang_comunicacion = 0;
if (e_randb_get_data_uart2(&data)){
//actualizar tabla comun
if((data.bearing > -2*PI) && (data.bearing < 2*PI)){
switch(data.data)
{
case 0:
comunicacionAngulos[0]=data.bearing;
comunicacionRangos[0]=data.range;
sprintf(tmp2,"Sigue Lider. Ang: %f, Rango: %f \n", data.bearing, data.range);
btcomSendString(tmp2);
break;
case 1:
comunicacionAngulos[1]=data.bearing;
comunicacionRangos[1]=data.range;
sprintf(tmp2,"Sigue Sucker. Ang: %f, Rango: %f \n", data.bearing, data.range);
btcomSendString(tmp2);
break;
}
}
}
switch(CURRENT_STATE){
case 0: //Collission
sprintf(tmp2,"-- COLISION max= %d --\n",maxProx);
btcomSendString(tmp2);
/* Calc vector Sum */
vector_repelent[0] = 0.0;
vector_repelent[1] = 0.0;
for (i = 0 ; i < 8; i ++ )
{
vector_repelent[0] += prox_reading[i] * cos ( prox_directions[i] );
vector_repelent[1] += prox_reading[i] * sin ( prox_directions[i] );
}
/* Calc pointing angle */
ang_repelent = atan2(vector_repelent[1], vector_repelent[0]);
/* Create repelent angle */
ang_repelent -= PI;
//calculate and set velocity
setAngularVelocity(ang_repelent,1);
break; // Case 0
case 1: // Chasing
sprintf(tmp2,"-- PERSIGUIENDO--\n");
btcomSendString(tmp2);
ang_comunicacion = (comunicacionAngulos[0] + comunicacionAngulos[1])/2;
if(ang_comunicacion>0.6 || ang_comunicacion<-0.6){ // Chasing
sprintf(tmp2,"-- GIRANDO ang= %02f --\n",ang_comunicacion);
btcomSendString(tmp2);
//calculate and set velocity
setAngularVelocity(ang_comunicacion,2);
}
else { // Walk
sprintf(tmp2,"-- RECTO--\n");
btcomSendString(tmp2);
e_set_speed_left(SPEED);
e_set_speed_right(SPEED);
}
break; // Case 1
} // End switch
//if (e_randb_get_data_uart2(&data)){
// sprintf(tmp,"%d %02f %d %2f %d\n",debug_var, data.bearing, data.range);
// btcomSendString(tmp);
/* Send the data through one sensor */
//e_randb_uart_send_all_data(data);
}
return 0;
}
