void ParticleSystemProcessSW::process(const ParticleSystemSW *p_system,const Transform& p_transform,float p_time) {
valid=false;
if (p_system->amount<=0) {
ERR_EXPLAIN("Invalid amount of particles: "+itos(p_system->amount));
ERR_FAIL_COND(p_system->amount<=0);
}
if (p_system->attractor_count<0 || p_system->attractor_count>VS::MAX_PARTICLE_ATTRACTORS) {
ERR_EXPLAIN("Invalid amount of particle attractors.");
ERR_FAIL_COND(p_system->attractor_count<0 || p_system->attractor_count>VS::MAX_PARTICLE_ATTRACTORS);
}
float lifetime = p_system->particle_vars[VS::PARTICLE_LIFETIME];
if (lifetime<CMP_EPSILON) {
ERR_EXPLAIN("Particle system lifetime too small.");
ERR_FAIL_COND(lifetime<CMP_EPSILON);
}
valid=true;
int particle_count=MIN(p_system->amount,ParticleSystemSW::MAX_PARTICLES);;
int emission_point_count = p_system->emission_points.size();
DVector<Vector3>::Read r;
if (emission_point_count)
r=p_system->emission_points.read();
if (particle_count!=particle_data.size()) {
//clear the whole system if particle amount changed
particle_data.clear();
particle_data.resize(p_system->amount);
particle_system_time=0;
}
float next_time = particle_system_time+p_time;
if (next_time > lifetime)
next_time=Math::fmod(next_time,lifetime);
ParticleData *pdata=&particle_data[0];
Vector3 attractor_positions[VS::MAX_PARTICLE_ATTRACTORS];
for(int i=0;i<p_system->attractor_count;i++) {
attractor_positions[i]=p_transform.xform(p_system->attractors[i].pos);
}
for(int i=0;i<particle_count;i++) {
ParticleData &p=pdata[i];
float restart_time = (i * lifetime / p_system->amount);
bool restart=false;
if ( next_time < particle_system_time ) {
if (restart_time > particle_system_time || restart_time < next_time )
restart=true;
} else if (restart_time > particle_system_time && restart_time < next_time ) {
restart=true;
}
if (restart) {
if (p_system->emitting) {
if (emission_point_count==0) { //use AABB
if (p_system->local_coordinates)
p.pos = p_system->emission_half_extents * Vector3( _rand_from_seed(&rand_seed), _rand_from_seed(&rand_seed), _rand_from_seed(&rand_seed) );
else
p.pos = p_transform.xform( p_system->emission_half_extents * Vector3( _rand_from_seed(&rand_seed), _rand_from_seed(&rand_seed), _rand_from_seed(&rand_seed) ) );
} else {
//use preset positions
if (p_system->local_coordinates)
p.pos = r[_irand_from_seed(&rand_seed)%emission_point_count];
else
p.pos = p_transform.xform( r[_irand_from_seed(&rand_seed)%emission_point_count] );
}
float angle1 = _rand_from_seed(&rand_seed)*p_system->particle_vars[VS::PARTICLE_SPREAD]*Math_PI;
float angle2 = _rand_from_seed(&rand_seed)*20.0*Math_PI; // make it more random like
Vector3 rot_xz=Vector3( Math::sin(angle1), 0.0, Math::cos(angle1) );
Vector3 rot = Vector3( Math::cos(angle2)*rot_xz.x,Math::sin(angle2)*rot_xz.x, rot_xz.z);
p.vel=(rot*p_system->particle_vars[VS::PARTICLE_LINEAR_VELOCITY]+rot*p_system->particle_randomness[VS::PARTICLE_LINEAR_VELOCITY]*_rand_from_seed(&rand_seed));
if (!p_system->local_coordinates)
p.vel=p_transform.basis.xform( p.vel );
p.vel+=p_system->emission_base_velocity;
p.rot=p_system->particle_vars[VS::PARTICLE_INITIAL_ANGLE]+p_system->particle_randomness[VS::PARTICLE_INITIAL_ANGLE]*_rand_from_seed(&rand_seed);
p.active=true;
for(int r=0;r<PARTICLE_RANDOM_NUMBERS;r++)
p.random[r]=_rand_from_seed(&rand_seed);
} else {
p.pos=Vector3();
p.rot=0;
p.vel=Vector3();
p.active=false;
}
} else {
if (!p.active)
continue;
Vector3 force;
//apply gravity
force=p_system->gravity_normal * (p_system->particle_vars[VS::PARTICLE_GRAVITY]+(p_system->particle_randomness[VS::PARTICLE_GRAVITY]*p.random[0]));
//apply linear acceleration
force+=p.vel.normalized() * (p_system->particle_vars[VS::PARTICLE_LINEAR_ACCELERATION]+p_system->particle_randomness[VS::PARTICLE_LINEAR_ACCELERATION]*p.random[1]);
//apply radial acceleration
Vector3 org;
if (!p_system->local_coordinates)
org=p_transform.origin;
force+=(p.pos-org).normalized() * (p_system->particle_vars[VS::PARTICLE_RADIAL_ACCELERATION]+p_system->particle_randomness[VS::PARTICLE_RADIAL_ACCELERATION]*p.random[2]);
//apply tangential acceleration
force+=(p.pos-org).cross(p_system->gravity_normal).normalized() * (p_system->particle_vars[VS::PARTICLE_TANGENTIAL_ACCELERATION]+p_system->particle_randomness[VS::PARTICLE_TANGENTIAL_ACCELERATION]*p.random[3]);
//apply attractor forces
for(int a=0;a<p_system->attractor_count;a++) {
force+=(p.pos-attractor_positions[a]).normalized() * p_system->attractors[a].force;
}
p.vel+=force * p_time;
if (p_system->particle_vars[VS::PARTICLE_DAMPING]) {
float v = p.vel.length();
float damp = p_system->particle_vars[VS::PARTICLE_DAMPING] + p_system->particle_vars[VS::PARTICLE_DAMPING] * p_system->particle_randomness[VS::PARTICLE_DAMPING];
v -= damp * p_time;
if (v<0) {
p.vel=Vector3();
} else {
p.vel=p.vel.normalized() * v;
}
}
p.rot+=(p_system->particle_vars[VS::PARTICLE_ANGULAR_VELOCITY]+p_system->particle_randomness[VS::PARTICLE_ANGULAR_VELOCITY]*p.random[4]) *p_time;
p.pos+=p.vel * p_time;
}
}
particle_system_time=Math::fmod( particle_system_time+p_time, lifetime );
}
void ClientApplication::initEntities()
{
Entity* entity = NULL;
Component* component = NULL;
// Read from assemblage
AssemblageHelper::E_FileStatus status = AssemblageHelper::FileStatus_OK;
EntityFactory* factory = static_cast<EntityFactory*>
( m_world->getSystem( SystemType::EntityFactory ) );
status = factory->readAssemblageFile( "Assemblages/testSpotLight.asd" );
EntitySystem* tempSys = NULL;
tempSys = m_world->getSystem(SystemType::GraphicsBackendSystem);
GraphicsBackendSystem* graphicsBackend = static_cast<GraphicsBackendSystem*>(tempSys);
/************************************************************************/
/* Create the main camera used to render the scene */
/************************************************************************/
entity = m_world->createEntity();
entity->setName("MainCamera");
entity->addComponent( new CameraInfo( m_world->getAspectRatio(),1.3f,1.0f,3000.0f ) );
entity->addComponent( new MainCamera_TAG() );
entity->addComponent( new AudioListener() );
entity->addComponent( new Transform( 0.0f, 0.0f, 0.0f ) );
m_world->addEntity(entity);
/************************************************************************/
/* Create shadow camera and spotlight. */
/************************************************************************/
float rotation = 0.78;
AglQuaternion quat;
for(int i = 0; i < 1; i++){
entity = factory->entityFromRecipe( "SpotLight" );
LightsComponent* lightComp = static_cast<LightsComponent*>(
entity->getComponent(ComponentType::LightsComponent));
int shadowIdx = -1;
vector<Light>* lights = lightComp->getLightsPtr();
for (unsigned int i = 0; i < lights->size(); i++){
if(lights->at(i).instanceData.shadowIdx != -1){
shadowIdx = graphicsBackend->getGfxWrapper()->generateShadowMap();
lights->at(i).instanceData.shadowIdx = shadowIdx;
}
}
Transform* transform = static_cast<Transform*>(
entity->getComponent(ComponentType::Transform));
quat = AglQuaternion::constructFromAxisAndAngle(AglVector3::up(),rotation);
transform->setRotation(quat);
CameraInfo* cameraInfo = new CameraInfo(1);
cameraInfo->m_shadowMapIdx = shadowIdx;
entity->addComponent(ComponentType::CameraInfo, cameraInfo);
entity->addTag(ComponentType::TAG_ShadowCamera, new ShadowCamera_TAG());
m_world->addEntity( entity );
rotation -= 0.78;
}
}
Vector3 CameraComponent::GetRotation()
{
Transform* tr = static_cast<Transform*>(GetSibling(CT_Transform));
return tr->GetRotation();
}
void projectVector( Vec& v )
{
v = transform.projectVector(v);
}
int BasicSceneExample::execute()
{
roll = 0.0f;
while (isOpen())
{
Vector<3, float> movement(0.0f, 0.0f, 0.0f);
const float moveSpeed = 0.06f;
if (moveForward->isTriggered())
movement += camera.getLook() * moveSpeed;
if (moveBack->isTriggered())
movement -= camera.getLook() * moveSpeed;
if (strafeLeft->isTriggered())
movement -= camera.getRight() * moveSpeed;
if (strafeRight->isTriggered())
movement += camera.getRight() * moveSpeed;
const float rollSpeed = 2.0f;
if (rollCCW->isTriggered())
roll += rollSpeed;
if (rollCW->isTriggered())
roll -= rollSpeed;
if (object->getSkeleton() && object->getSkeleton()->isLoaded())
{
// Pose
const Skeleton* skeleton = object->getSkeleton().get();
for (std::size_t i = 0; i < skeleton->getBoneCount(); ++i)
{
pose->setRelativeTransform(i, skeleton->getBindPose()->getRelativeTransform(i));
}
Quaternion<float> leftFemurRotation, leftTibiaRotation, rightFemurRotation, rightTibiaRotation;
leftFemurRotation = Quaternion<float>::fromAxisAngle({0.0f, 1.0f, 0.0f}, math::radians<float>(roll * 0.25f));
leftTibiaRotation = Quaternion<float>::fromAxisAngle({0.0f, 1.0f, 0.0f}, math::radians<float>(roll));
rightFemurRotation = Quaternion<float>::fromAxisAngle({0.0f, 0.0f, 1.0f}, math::radians<float>(45.0f));
rightTibiaRotation = Quaternion<float>::fromAxisAngle({0.0f, 0.0f, 1.0f}, math::radians<float>(-75.0f));
const Bone* bone = skeleton->getBone("anterior_abdomen");
if (bone)
{
std::size_t index = bone->getIndex();
Transform<float> transform = pose->getRelativeTransform(index);
transform.setRotation(transform.getRotation() * leftTibiaRotation);
pose->setRelativeTransform(index, transform);
}
bone = skeleton->getBone("posterior_abdomen");
if (bone)
{
std::size_t index = bone->getIndex();
Transform<float> transform = pose->getRelativeTransform(index);
transform.setRotation(transform.getRotation() * leftFemurRotation);
pose->setRelativeTransform(index, transform);
}
/*
bone = skeleton->getBone("right_midleg_femur");
if (bone)
{
std::size_t index = bone->getIndex();
Transform<float> transform = pose->getRelativeTransform(index);
transform.setRotation(transform.getRotation() * rightFemurRotation);
pose->setRelativeTransform(index, transform);
}
bone = skeleton->getBone("right_midleg_tibia");
if (bone)
{
std::size_t index = bone->getIndex();
Transform<float> transform = pose->getRelativeTransform(index);
transform.setRotation(transform.getRotation() * rightTibiaRotation);
pose->setRelativeTransform(index, transform);
}
*/
pose->concatenate();
}
if (0 && roll != 0.0f)
{
Quaternion<float> rotation = Quaternion<float>::fromAxisAngle(camera.getLook(), roll).normalized();
up = rotation * up;
updateCamera();
}
camera.setAspectRatio(window->getViewport().getAspectRatio());
camera.lookAt(
camera.getPosition() + movement,
camera.getPosition() + movement + camera.getLook(),
camera.getUp());
camera.update();
float paramInc = 0.004f;
if (increaseAction->isTriggered())
{
layerParam->setRoughness(std::min(1.0f, layerParam->getRoughness() + paramInc));
std::cout << "roughness: " << layerParam->getRoughness() << '\n';
}
if (decreaseAction->isTriggered())
{
layerParam->setRoughness(std::max(0.0f, layerParam->getRoughness() - paramInc));
std::cout << "roughness: " << layerParam->getRoughness() << '\n';
}
//light.setPosition(camera.getPosition());
//light.setDirection(camera.getLook());
//plight.setPosition(light.getPosition());
//sun.setDirection(camera.getLook());
graphicsContext->setViewport(window->getViewport());
graphicsContext->clear(ClearOption::COLOR_BUFFER | ClearOption::DEPTH_BUFFER);
renderer->render(&scene, &camera);
renderer->render(&gui, &orthoCamera);
window->swapBuffers();
}
return EXIT_SUCCESS;
}
Vector<Plane> CameraMatrix::get_projection_planes(const Transform& p_transform) const {
/** Fast Plane Extraction from combined modelview/projection matrices.
* References:
* http://www.markmorley.com/opengl/frustumculling.html
* http://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf
*/
Vector<Plane> planes;
const float * matrix = (const float*)this->matrix;
Plane new_plane;
///////--- Near Plane ---///////
new_plane=Plane(matrix[ 3] + matrix[ 2],
matrix[ 7] + matrix[ 6],
matrix[11] + matrix[10],
matrix[15] + matrix[14]);
new_plane.normal=-new_plane.normal;
new_plane.normalize();
planes.push_back( p_transform.xform(new_plane) );
///////--- Far Plane ---///////
new_plane=Plane(matrix[ 3] - matrix[ 2],
matrix[ 7] - matrix[ 6],
matrix[11] - matrix[10],
matrix[15] - matrix[14]);
new_plane.normal=-new_plane.normal;
new_plane.normalize();
planes.push_back( p_transform.xform(new_plane) );
///////--- Left Plane ---///////
new_plane=Plane(matrix[ 3] + matrix[ 0],
matrix[ 7] + matrix[ 4],
matrix[11] + matrix[ 8],
matrix[15] + matrix[12]);
new_plane.normal=-new_plane.normal;
new_plane.normalize();
planes.push_back( p_transform.xform(new_plane) );
///////--- Top Plane ---///////
new_plane=Plane(matrix[ 3] - matrix[ 1],
matrix[ 7] - matrix[ 5],
matrix[11] - matrix[ 9],
matrix[15] - matrix[13]);
new_plane.normal=-new_plane.normal;
new_plane.normalize();
planes.push_back( p_transform.xform(new_plane) );
///////--- Right Plane ---///////
new_plane=Plane(matrix[ 3] - matrix[ 0],
matrix[ 7] - matrix[ 4],
matrix[11] - matrix[ 8],
matrix[15] - matrix[12]);
new_plane.normal=-new_plane.normal;
new_plane.normalize();
planes.push_back( p_transform.xform(new_plane) );
///////--- Bottom Plane ---///////
new_plane=Plane(matrix[ 3] + matrix[ 1],
matrix[ 7] + matrix[ 5],
matrix[11] + matrix[ 9],
matrix[15] + matrix[13]);
new_plane.normal=-new_plane.normal;
new_plane.normalize();
planes.push_back( p_transform.xform(new_plane) );
return planes;
}
void DefaultGeometry::generateDecorations
(const State& state,
Array_<SimTK::DecorativeGeometry>& geometry)
{
const SimbodyMatterSubsystem& matter = _model.getMatterSubsystem();
const ModelDisplayHints& hints = _model.getDisplayHints();
// Display wrap objects.
if (hints.get_show_wrap_geometry()) {
const Vec3 color(SimTK::Cyan);
Transform ztoy;
ztoy.updR().setRotationFromAngleAboutX(SimTK_PI/2);
const BodySet& bodies = _model.getBodySet();
for (int i = 0; i < bodies.getSize(); i++) {
const OpenSim::Body& body = bodies[i];
const Transform& X_GB =
body.getMobilizedBody().getBodyTransform(state);
const WrapObjectSet& wrapObjects = body.getWrapObjectSet();
for (int j = 0; j < wrapObjects.getSize(); j++) {
const string type = wrapObjects[j].getConcreteClassName();
if (type == "WrapCylinder") {
const WrapCylinder* cylinder =
dynamic_cast<const WrapCylinder*>(&wrapObjects[j]);
if (cylinder != NULL) {
Transform X_GW = X_GB*cylinder->getTransform()*ztoy;
geometry.push_back(
DecorativeCylinder(cylinder->getRadius(),
cylinder->getLength()/2)
.setTransform(X_GW).setResolution(_dispWrapResolution)
.setColor(color).setOpacity(_dispWrapOpacity));
}
}
else if (type == "WrapEllipsoid") {
const WrapEllipsoid* ellipsoid =
dynamic_cast<const WrapEllipsoid*>(&wrapObjects[j]);
if (ellipsoid != NULL) {
Transform X_GW = X_GB*ellipsoid->getTransform();
geometry.push_back(
DecorativeEllipsoid(ellipsoid->getRadii())
.setTransform(X_GW).setResolution(_dispWrapResolution)
.setColor(color).setOpacity(_dispWrapOpacity));
}
}
else if (type == "WrapSphere") {
const WrapSphere* sphere =
dynamic_cast<const WrapSphere*>(&wrapObjects[j]);
if (sphere != NULL) {
Transform X_GW = X_GB*sphere->getTransform();
geometry.push_back(
DecorativeSphere(sphere->getRadius())
.setTransform(X_GW).setResolution(_dispWrapResolution)
.setColor(color).setOpacity(_dispWrapOpacity));
}
}
}
}
}
// Display contact geometry objects.
if (hints.get_show_contact_geometry()) {
const Vec3 color(SimTK::Green);
Transform ztoy;
ztoy.updR().setRotationFromAngleAboutX(SimTK_PI/2);
const ContactGeometrySet& contactGeometries = _model.getContactGeometrySet();
for (int i = 0; i < contactGeometries.getSize(); i++) {
const PhysicalFrame& body = contactGeometries.get(i).getBody();
const Transform& X_GB =
matter.getMobilizedBody(body.getMobilizedBodyIndex()).getBodyTransform(state);
const string type = contactGeometries.get(i).getConcreteClassName();
const int displayPref = contactGeometries.get(i).getDisplayPreference();
//cout << type << ": " << contactGeometries.get(i).getName() << ": disp pref = " << displayPref << endl;
if (type == "ContactSphere" && displayPref == 4) {
ContactSphere* sphere =
dynamic_cast<ContactSphere*>(&contactGeometries.get(i));
if (sphere != NULL) {
Transform X_GW = X_GB*sphere->getTransform();
geometry.push_back(
DecorativeSphere(sphere->getRadius())
.setTransform(X_GW).setResolution(_dispContactResolution)
.setColor(color).setOpacity(_dispContactOpacity));
}
}
}
}
// Ask all the ModelComponents to generate dynamic geometry.
_model.generateDecorations(false, _model.getDisplayHints(),
state, geometry);
}
void BodySW::integrate_velocities(real_t p_step) {
if (mode == PhysicsServer::BODY_MODE_STATIC)
return;
if (fi_callback)
get_space()->body_add_to_state_query_list(&direct_state_query_list);
if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
_set_transform(new_transform, false);
_set_inv_transform(new_transform.affine_inverse());
if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3())
set_active(false); //stopped moving, deactivate
return;
}
//apply axis lock
if (axis_lock != PhysicsServer::BODY_AXIS_LOCK_DISABLED) {
int axis = axis_lock - 1;
for (int i = 0; i < 3; i++) {
if (i == axis) {
linear_velocity[i] = 0;
biased_linear_velocity[i] = 0;
} else {
angular_velocity[i] = 0;
biased_angular_velocity[i] = 0;
}
}
}
Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity;
real_t ang_vel = total_angular_velocity.length();
Transform transform = get_transform();
if (ang_vel != 0.0) {
Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
Basis rot(ang_vel_axis, ang_vel * p_step);
Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local);
transform.basis = rot * transform.basis;
transform.orthonormalize();
}
Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity;
/*for(int i=0;i<3;i++) {
if (axis_lock&(1<<i)) {
transform.origin[i]=0.0;
}
}*/
transform.origin += total_linear_velocity * p_step;
_set_transform(transform);
_set_inv_transform(get_transform().inverse());
_update_transform_dependant();
/*
if (fi_callback) {
get_space()->body_add_to_state_query_list(&direct_state_query_list);
*/
}
void Init(CompositeNode* root, MeshFileLoader* MeshLoader )
{
// glDisable(GL_CULL_FACE);
// glEnable(GL_CULL_FACE);
// glCullFace(GL_BACK);
// m = mfl->Load("Objects/cube.obj");
// g = new Geometry(m, "Geometry");
// g->SetMesh( m );
Mesh* Magnolia = MeshLoader->Load("Objects/magnolia.obj");
Magnolia->Scale(0.03f);
Mesh* Rose = MeshLoader->Load("Objects/rose+vase.obj");
Rose->Scale(0.03f);
//Mesh* Dolphins = MeshLoader->Load("Objects/dolphins.obj");
//Dolphins->Scale(0.01f);
Mesh* Skyscraper = MeshLoader->Load("Objects/skyscraper.obj");
Skyscraper->Scale(0.04f);
M3DVector3f pos;
m3dLoadVector3( pos, 0.0f, 0.0f, 50.0f );
if (_camera) DLOG(INFO) << "_camera address: " << _camera << endl;
_camera->SetPosition( pos );
Vector4 color( 1.0f, 1.0f, 1.0f, 1.0f );
_light->SetDiffuse( color );
_light->SetAmbient( color );
// M3DVector3f pos;
// m3dLoadVector3( pos, 0.0f, 0.0f, 10.0f );
// Init Camera
// Camera* c = reinterpret_cast<Camera*>(l->GetByName("GlobalCamera"));
// c->SetPosition( pos );
// c->SetPerspective(45.0f,(GLfloat)800/(GLfloat)600,0.1f,100.0f);
// Build Up Scenegraph
_camera->AddChild(
(new Transform(string("MagnoliaTransform")))->AddChild(
(new Geometry(Magnolia, string("Magnolia")))
));
_camera->AddChild(
(new Transform(string("RoseTransform")))->AddChild(
(new Geometry(Rose, "Rose")))
);
_camera->AddChild(
(new Transform(string("SkyscraperTransform")))->AddChild(
(new Geometry(Skyscraper, "Skyscraper")))
);
Transform* t = dynamic_cast<Transform*>(root->GetByName("MagnoliaTransform"));
t->Rotate(20.0f, 1.0f, 1.0f, 1.0f);
t->Translate(-3.0f, 0.0f, 0.0f);
t = dynamic_cast<Transform*>(root->GetByName("RoseTransform"));
t->Translate(2.0f, 0.0f, 0.0f);
t->Rotate(20.0f, 1.0f, 1.0f, 1.0f);
t->Scale(1.6f, 1.2f, 1.2f);
t = dynamic_cast<Transform*>(root->GetByName("SkyscraperTransform"));
t->Rotate(50.0f, 1.0f, 1.0f, 1.0f);
update = t;
UpdateVisitorFactory fact;
visitor = fact.CreateTransformationVisitor( ROTATE, 1.0f, 0.0f, 0.0f, 0.01f );
visitor2 = fact.CreateTransformationVisitor( SCALE, 1.002f, 1.0f, 1.0f );
};
bool Tween::_calc_delta_val(const Variant& p_initial_val, const Variant& p_final_val, Variant& p_delta_val) {
const Variant& initial_val = p_initial_val;
const Variant& final_val = p_final_val;
Variant& delta_val = p_delta_val;
switch(initial_val.get_type()) {
case Variant::BOOL:
//delta_val = p_final_val;
delta_val = (int) p_final_val - (int) p_initial_val;
break;
case Variant::INT:
delta_val = (int) final_val - (int) initial_val;
break;
case Variant::REAL:
delta_val = (real_t) final_val - (real_t) initial_val;
break;
case Variant::VECTOR2:
delta_val = final_val.operator Vector2() - initial_val.operator Vector2();
break;
case Variant::VECTOR3:
delta_val = final_val.operator Vector3() - initial_val.operator Vector3();
break;
case Variant::MATRIX3:
{
Matrix3 i = initial_val;
Matrix3 f = final_val;
delta_val = Matrix3(f.elements[0][0] - i.elements[0][0],
f.elements[0][1] - i.elements[0][1],
f.elements[0][2] - i.elements[0][2],
f.elements[1][0] - i.elements[1][0],
f.elements[1][1] - i.elements[1][1],
f.elements[1][2] - i.elements[1][2],
f.elements[2][0] - i.elements[2][0],
f.elements[2][1] - i.elements[2][1],
f.elements[2][2] - i.elements[2][2]
);
}
break;
case Variant::MATRIX32:
{
Matrix32 i = initial_val;
Matrix32 f = final_val;
Matrix32 d = Matrix32();
d[0][0] = f.elements[0][0] - i.elements[0][0];
d[0][1] = f.elements[0][1] - i.elements[0][1];
d[1][0] = f.elements[1][0] - i.elements[1][0];
d[1][1] = f.elements[1][1] - i.elements[1][1];
d[2][0] = f.elements[2][0] - i.elements[2][0];
d[2][1] = f.elements[2][1] - i.elements[2][1];
delta_val = d;
}
break;
case Variant::QUAT:
delta_val = final_val.operator Quat() - initial_val.operator Quat();
break;
case Variant::_AABB:
{
AABB i = initial_val;
AABB f = final_val;
delta_val = AABB(f.pos - i.pos, f.size - i.size);
}
break;
case Variant::TRANSFORM:
{
Transform i = initial_val;
Transform f = final_val;
Transform d;
d.set(f.basis.elements[0][0] - i.basis.elements[0][0],
f.basis.elements[0][1] - i.basis.elements[0][1],
f.basis.elements[0][2] - i.basis.elements[0][2],
f.basis.elements[1][0] - i.basis.elements[1][0],
f.basis.elements[1][1] - i.basis.elements[1][1],
f.basis.elements[1][2] - i.basis.elements[1][2],
f.basis.elements[2][0] - i.basis.elements[2][0],
f.basis.elements[2][1] - i.basis.elements[2][1],
f.basis.elements[2][2] - i.basis.elements[2][2],
f.origin.x - i.origin.x,
f.origin.y - i.origin.y,
f.origin.z - i.origin.z
);
delta_val = d;
}
break;
case Variant::COLOR:
{
Color i = initial_val;
Color f = final_val;
delta_val = Color(f.r - i.r, f.g - i.g, f.b - i.b, f.a - i.a);
}
break;
default:
ERR_PRINT("Invalid param type, except(int/real/vector2/vector/matrix/matrix32/quat/aabb/transform/color)");
return false;
};
return true;
}
void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) {
switch (p_state) {
case PhysicsServer::BODY_STATE_TRANSFORM: {
if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
new_transform = p_variant;
//wakeup_neighbours();
set_active(true);
if (first_time_kinematic) {
_set_transform(p_variant);
_set_inv_transform(get_transform().affine_inverse());
first_time_kinematic = false;
}
} else if (mode == PhysicsServer::BODY_MODE_STATIC) {
_set_transform(p_variant);
_set_inv_transform(get_transform().affine_inverse());
wakeup_neighbours();
} else {
Transform t = p_variant;
t.orthonormalize();
new_transform = get_transform(); //used as old to compute motion
if (new_transform == t)
break;
_set_transform(t);
_set_inv_transform(get_transform().inverse());
}
wakeup();
} break;
case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
/*
if (mode==PhysicsServer::BODY_MODE_STATIC)
break;
*/
linear_velocity = p_variant;
wakeup();
} break;
case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
/*
if (mode!=PhysicsServer::BODY_MODE_RIGID)
break;
*/
angular_velocity = p_variant;
wakeup();
} break;
case PhysicsServer::BODY_STATE_SLEEPING: {
//?
if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC)
break;
bool do_sleep = p_variant;
if (do_sleep) {
linear_velocity = Vector3();
//biased_linear_velocity=Vector3();
angular_velocity = Vector3();
//biased_angular_velocity=Vector3();
set_active(false);
} else {
if (mode != PhysicsServer::BODY_MODE_STATIC)
set_active(true);
}
} break;
case PhysicsServer::BODY_STATE_CAN_SLEEP: {
can_sleep = p_variant;
if (mode == PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep)
set_active(true);
} break;
}
}
void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
int vertex_count=mesh.vertices.size();
if (vertex_count==0)
return;
const Vector3 *vrts=&mesh.vertices[0];
#ifndef NEON
for (int i=0;i<vertex_count;i++) {
float d=p_normal.dot( p_transform.xform( vrts[i] ) );
if (i==0 || d > r_max)
r_max=d;
if (i==0 || d < r_min)
r_min=d;
}
#else
int i;
Matrix3 m = p_transform.get_basis();
Vector3 o = p_transform.get_origin();
float32x4_t vo[4] = {{o[0],o[0],o[0],o[0]} , {o[1],o[1],o[1],o[1]} , {o[2],o[2],o[2],o[2]} , {o[3],o[3],o[3],o[3]}};
for (i=0;i<vertex_count-4;i+=4) { // as long as 4 calculations at a time are possible
/*_FORCE_INLINE_ Vector3 Transform::xform(const Vector3& p_vector) const {
return Vector3(
basis[0].dot(p_vector)+origin.x,
basis[1].dot(p_vector)+origin.y,
basis[2].dot(p_vector)+origin.z
);
}*/
//print_line("yay");
//float d1, d2, d3, d4;
//float f1_1, f1_2, f1_3, f1_4, f2_1, f2_2, f2_3, f2_4, f3_1, f3_2, f3_3, f3_4;
float32x4_t f1, f2, f3;
float32x4_t d;
float32x4_t vrts_x = {vrts[i].x, vrts[i+1].x, vrts[i+2].x, vrts[i+3].x};
float32x4_t vrts_y = {vrts[i].y, vrts[i+1].y, vrts[i+2].y, vrts[i+3].y};
float32x4_t vrts_z = {vrts[i].z, vrts[i+1].z, vrts[i+2].z, vrts[i+3].z};
/*f1_1 = m[0][0]*vrts[i][0];
f1_2 = m[0][0]*vrts[i+1][0];
f1_3 = m[0][0]*vrts[i+2][0];
f1_4 = m[0][0]*vrts[i+3][0];*/
//f1 = vrts_x * m[0][0];
f1 = vmulq_n_f32(vrts_x, m[0][0]);
/*f2_1 = m[1][0]*vrts[i][0];
f2_2 = m[1][0]*vrts[i+1][0];
f2_3 = m[1][0]*vrts[i+2][0];
f2_4 = m[1][0]*vrts[i+3][0];*/
//f2 = m[1][0] * vrts_x;
f2 = vmulq_n_f32(vrts_x, m[1][0]);
/*f3_1 = m[2][0]*vrts[i][0];
f3_2 = m[2][0]*vrts[i+1][0];
f3_3 = m[2][0]*vrts[i+2][0];
f3_4 = m[2][0]*vrts[i+3][0];*/
//f3 = m[2][0] * vrts_x;
f3 = vmulq_n_f32(vrts_x, m[2][0]);
/*f1_1 += m[0][1]*vrts[i][1];
f1_2 += m[0][1]*vrts[i+1][1];
f1_3 += m[0][1]*vrts[i+2][1];
f1_4 += m[0][1]*vrts[i+3][1];*/
//f1 += m[0][1] * vrts_y;
f1 += vmulq_n_f32(vrts_y, m[0][1]);
/*f2_1 += m[1][1]*vrts[i][1];
f2_2 += m[1][1]*vrts[i+1][1];
f2_3 += m[1][1]*vrts[i+2][1];
f2_4 += m[1][1]*vrts[i+3][1];*/
//f2 += m[1][1] * vrts_y;
f2 += vmulq_n_f32(vrts_y, m[1][1]);
/*f3_1 += m[2][1]*vrts[i][1];
f3_2 += m[2][1]*vrts[i+1][1];
f3_3 += m[2][1]*vrts[i+2][1];
f3_4 += m[2][1]*vrts[i+3][1];*/
//f3 += m[2][1] * vrts_y;
f3 += vmulq_n_f32(vrts_y, m[2][1]);
/*f1_1 += m[0][2]*vrts[i][2];
f1_2 += m[0][2]*vrts[i+1][2];
f1_3 += m[0][2]*vrts[i+2][2];
f1_4 += m[0][2]*vrts[i+3][2];*/
//f1 += m[0][2] * vrts_z;
f1 += vmulq_n_f32(vrts_z, m[0][2]);
/*f2_1 += m[1][2]*vrts[i][2];
f2_2 += m[1][2]*vrts[i+1][2];
f2_3 += m[1][2]*vrts[i+2][2];
f2_4 += m[1][2]*vrts[i+3][2];*/
//f2 += m[1][2] * vrts_z;
f2 += vmulq_n_f32(vrts_z, m[1][2]);
/*f3_1 += m[2][2]*vrts[i][2];
f3_2 += m[2][2]*vrts[i+1][2];
f3_3 += m[2][2]*vrts[i+2][2];
f3_4 += m[2][2]*vrts[i+3][2];*/
//f3 += m[2][2] * vrts_z;
f3 += vmulq_n_f32(vrts_z, m[2][2]);
/*f1_1 += o[0];
f1_2 += o[0];
f1_3 += o[0];
f1_4 += o[0];*/
f1 += vo[0];
/*f2_1 += o[1];
f2_2 += o[1];
f2_3 += o[1];
f2_4 += o[1];*/
f2 += vo[1];
/*f3_1 += o[2];
f3_2 += o[2];
f3_3 += o[2];
f3_4 += o[2];*/
f3 += vo[2];
/*d1 = f1_1*p_normal[0];
d2 = f1_2*p_normal[0];
d3 = f1_3*p_normal[0];
d4 = f1_4*p_normal[0];*/
d = vmulq_n_f32(f1 , p_normal[0]);
/*d1 += f2_1*p_normal[1];
d2 += f2_2*p_normal[1];
d3 += f2_3*p_normal[1];
d4 += f2_4*p_normal[1];*/
d += vmulq_n_f32(f2 , p_normal[1]);
/*d1 += f3_1*p_normal[2];
d2 += f3_2*p_normal[2];
d3 += f3_3*p_normal[2];
d4 += f3_4*p_normal[2];*/
d += vmulq_n_f32(f3 , p_normal[2]);
float *fd = (float *)&d;
if (i==0 || fd[0] > r_max)
r_max=fd[0];
if (i==0 || fd[0] < r_min)
r_min=fd[0];
if (i==0 || fd[1] > r_max)
r_max=fd[1];
if (i==0 || fd[1] < r_min)
r_min=fd[1];
if (i==0 || fd[2] > r_max)
r_max=fd[2];
if (i==0 || fd[2] < r_min)
r_min=fd[2];
if (i==0 || fd[3] > r_max)
r_max=fd[3];
if (i==0 || fd[3] < r_min)
r_min=fd[3];
}
for (i=i;i<vertex_count;i++) { // rest
/*_FORCE_INLINE_ Vector3 Transform::xform(const Vector3& p_vector) const {
return Vector3(
basis[0].dot(p_vector)+origin.x,
basis[1].dot(p_vector)+origin.y,
basis[2].dot(p_vector)+origin.z
);
}*/
float d;
Matrix3 m = p_transform.get_basis();
Vector3 o = p_transform.get_origin();
float f1, f2, f3;
f1 = m[0][0]*vrts[i][0];
f2 = m[1][0]*vrts[i][0];
f3 = m[2][0]*vrts[i][0];
f1 += m[0][1]*vrts[i][1];
f2 += m[1][1]*vrts[i][1];
f3 += m[2][1]*vrts[i][1];
f1 += m[0][2]*vrts[i][2];
f2 += m[1][2]*vrts[i][2];
f3 += m[2][2]*vrts[i][2];
f1 += o[0];
f2 += o[1];
f3 += o[2];
d = f1*p_normal[0];
d += f2*p_normal[1];
d += f3*p_normal[2];
if (i==0 || d > r_max)
r_max=d;
if (i==0 || d < r_min)
r_min=d;
}
#endif
}
void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
//not very useful, but not very used either
p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max );
}
void ParticleSystemDrawInfoSW::prepare(const ParticleSystemSW *p_system,const ParticleSystemProcessSW *p_process,const Transform& p_system_transform,const Transform& p_camera_transform) {
ERR_FAIL_COND(p_process->particle_data.size() != p_system->amount);
ERR_FAIL_COND(p_system->amount<=0 || p_system->amount>=ParticleSystemSW::MAX_PARTICLES);
const ParticleSystemProcessSW::ParticleData *pdata=&p_process->particle_data[0];
float time_pos=p_process->particle_system_time/p_system->particle_vars[VS::PARTICLE_LIFETIME];
ParticleSystemSW::ColorPhase cphase[VS::MAX_PARTICLE_COLOR_PHASES];
float last=-1;
int col_count=0;
for(int i=0;i<p_system->color_phase_count;i++) {
if (p_system->color_phases[i].pos<=last)
break;
cphase[i]=p_system->color_phases[i];
col_count++;
}
Vector3 camera_z_axis = p_camera_transform.basis.get_axis(2);
for(int i=0;i<p_system->amount;i++) {
ParticleDrawInfo &pdi=draw_info[i];
pdi.data=&pdata[i];
pdi.transform.origin=pdi.data->pos;
if (p_system->local_coordinates)
pdi.transform.origin=p_system_transform.xform(pdi.transform.origin);
pdi.d=-camera_z_axis.dot(pdi.transform.origin);
// adjust particle size, color and rotation
float time = ((float)i / p_system->amount);
if (time<time_pos)
time=time_pos-time;
else
time=(1.0-time)+time_pos;
Vector3 up=p_camera_transform.basis.get_axis(1); // up determines the rotation
float up_scale=1.0;
if (p_system->height_from_velocity) {
Vector3 veld = pdi.data->vel;
Vector3 cam_z = camera_z_axis.normalized();
float vc = Math::abs(veld.normalized().dot(cam_z));
if (vc<(1.0-CMP_EPSILON)) {
up = Plane(cam_z,0).project(veld).normalized();
float h = p_system->particle_vars[VS::PARTICLE_HEIGHT]+p_system->particle_randomness[VS::PARTICLE_HEIGHT]*pdi.data->random[7];
float velh = veld.length();
h+=velh*(p_system->particle_vars[VS::PARTICLE_HEIGHT_SPEED_SCALE]+p_system->particle_randomness[VS::PARTICLE_HEIGHT_SPEED_SCALE]*pdi.data->random[7]);
up_scale=Math::lerp(1.0,h,(1.0-vc));
}
} else if (pdi.data->rot) {
up.rotate(camera_z_axis,pdi.data->rot);
}
{
// matrix
Vector3 v_z = (p_camera_transform.origin-pdi.transform.origin).normalized();
// Vector3 v_z = (p_camera_transform.origin-pdi.data->pos).normalized();
Vector3 v_y = up;
Vector3 v_x = v_y.cross(v_z);
v_y = v_z.cross(v_x);
v_x.normalize();
v_y.normalize();
float initial_scale, final_scale;
initial_scale = p_system->particle_vars[VS::PARTICLE_INITIAL_SIZE]+p_system->particle_randomness[VS::PARTICLE_INITIAL_SIZE]*pdi.data->random[5];
final_scale = p_system->particle_vars[VS::PARTICLE_FINAL_SIZE]+p_system->particle_randomness[VS::PARTICLE_FINAL_SIZE]*pdi.data->random[6];
float scale = initial_scale + time * (final_scale - initial_scale);
pdi.transform.basis.set_axis(0,v_x * scale);
pdi.transform.basis.set_axis(1,v_y * scale * up_scale);
pdi.transform.basis.set_axis(2,v_z * scale);
}
int cpos=0;
while(cpos<col_count) {
if (cphase[cpos].pos > time)
break;
cpos++;
}
cpos--;
if (cpos==-1)
pdi.color=Color(1,1,1,1);
else {
if (cpos==col_count-1)
pdi.color=cphase[cpos].color;
else {
float diff = (cphase[cpos+1].pos-cphase[cpos].pos);
if (diff>0)
pdi.color=cphase[cpos].color.linear_interpolate(cphase[cpos+1].color, (time - cphase[cpos].pos) / diff );
else
pdi.color=cphase[cpos+1].color;
}
}
draw_info_order[i]=&pdi;
}
SortArray<ParticleDrawInfo*,_ParticleSorterSW> particle_sort;
particle_sort.sort(&draw_info_order[0],p_system->amount);
}
// Given a point Q on an ellipsoid, with outward unit normal nn at Q: find the
// principal curvatures at the point and their directions. The result is a
// coordinate frame with origin Q, z axis the ellipsoid normal nn at Q, x axis
// is the direction dmax of maximum curvature kmax, y axis the direction dmin
// of minimum curvature kmin, such that [dmax dmin n] forms a right-handed set.
// This is equivalent to fitting an elliptic paraboloid
// z = -kmax/2 x^2 -kmin/2 y^2 to the ellipsoid at point Q. Note that for
// an ellipsoid we have kmax>=kmin>0.
//
// We'll find the ellipse on the central plane perpendicular to the normal by
// intersecting the plane equation with the ellipsoid equation but working in
// the plane frame P=[u v n], where u and v are arbitrary axes in the plane.
// Our goal is to obtain an equation for the ellipse in P and then rotate the
// P frame about its normal until we get the ellipse in standard form
// Ru^2+Sv^2=1 in which case d/R and d/S are the ellipsoid curvatures (d is the
// distance from the point on the ellipsoid to the plane).
// ref: McArthur, Neil. "Principal radii of curvature at a point on an
// ellipsoid", Mathematical Notes 24 pp. xvi-xvii, 1929.
//
// In its own frame E=[x y z] the ellipsoid surface is the set of points such
// that
// ~e * diag(A,B,C) * e = 1
// where e is a vector expressed in E. The plane is the set of points
// satisfying ~e * n = 0. We can write rotation matrix R_EP=[u v n] where
// u,v,n are expressed in E. Now we can put the ellipsoid in P:
// ~(R_EP*p) * diag(A,B,C) * (R_EP*p) = 1
// We can intersect that with the plane just by dropping the n coordinate of
// p so p=[u v 0] (u,v scalars here), and the intersection equation is
// A(u*ux + v*vx)^2 + B(u*uy+v*vy)^2 + C(u*uz + v*vz)^2 = 1
// which is
// R u^2 + S v^2 + T u*v = 1
// with
// R = A ux^2 + B uy^2 + C uz^2
// S = A vx^2 + B vy^2 + C vz^2
// T = 2(A ux*vx + B uy*vy + C uz*vz)
//
// We want to find a rotation about n that eliminates the cross term Tuv,
// leaving us with
// R' u'^2 + S' v'^2 = 1
// for new constants R' and S' and new basis u' and v'.
//
// Method
// ------
// We'll calculate an angle theta where theta=0 would be along u and
// theta=pi/2 would be along v. Then theta+pi/2 is a perpendicular direction
// that has the other curvature extreme. Per "Dr Rob" at Mathforum.org 2000:
// t2t = tan(2*theta) = T/(R-S)
// theta = atan(t2t)/2, c = cos(theta), s = sin(theta)
// R' = Rc^2 + Tsc + Ss^2 (theta direction)
// S' = Rs^2 - Tsc + Sc^2 (theta+pi/2 direction)
// Directions are u' = c*u + s*v, v' = c*v - s*u; these are automatically unit
// vectors.
//
// Optimization
// ------------
// The above requires an atan() to get 2*theta then sin & cos(theta) at
// a cost of about 120 flops. We can use half angle formulas to work
// exclusively with 2*theta, but then we'll have to normalize u' and v'
// at the end:
// t2t = tan(2*theta) = T/(R-S)
// c2t = cos(2*theta) = 1/sqrt(1 + t2t^2)
// s2t = sin(2*theta) = t2t*cos2t;
// 2*R' = R+S + Rc2t - Sc2t + Ts2t
// 2*S' = R+S - Rc2t + Sc2t - Ts2t
// By multiplying the u',v' formulas above by 2*c we change the lengths
// but get expressions that are easily converted to double angles:
// u' = normalize((1+c2t)*u + s2t*v)
// v' = normalize((1+c2t)*v - s2t*u)
// (but actually v' is n X u' which is cheap). This saves about 30
// flops over the straightforward method above.
//
// Cost: given a point and normalized normal
// curvatures ~160 flops
// directions ~ 60 flops more
// ----
// ~220 flops
//
// So: Given an ellipsoid in its own frame E, with equation Ax^2+By^2+Cz^2=1, a
// point Q=(x,y,z) on its surface, and the unit outward normal vector nn at Q,
// return (kmax,kmin) the principal curvatures at Q, and a Transform with
// x=dmax, y=dmin, z=nn, O=Q that gives the principal curvature directions.
// (Note: A=1/a^2, B=1/b^2, C=1/c^2 where a,b,c are the ellipsoid radii.)
void ContactGeometry::Ellipsoid::Impl::
findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& nn,
Transform& X_EP, Vec2& k) const
{
const Real A = square(curvatures[0]), B = square(curvatures[1]),
C = square(curvatures[2]);
// Sanity checks in debug.
SimTK_ERRCHK(std::abs(A*Q[0]*Q[0]+B*Q[1]*Q[1]+C*Q[2]*Q[2]-1) < SqrtEps,
"ContactGeometry::Ellipsoid::findParaboloidAtPointWithNormal()",
"The given point was not on the surface of the ellipsoid.");
SimTK_ERRCHK((nn-findUnitNormalAtPoint(Q)).normSqr() < SqrtEps,
"ContactGeometry::Ellipsoid::findParaboloidAtPointWithNormal()",
"The given normal was not consistent with the given point.");
UnitVec3 tu = nn.perp(); // ~40 flops
UnitVec3 tv(nn % tu, true); // y = z X x for plane, already normalized (9 flops)
// 27 flops to get R,S,T
Real R= A*square(tu[0]) + B*square(tu[1]) + C*square(tu[2]);
Real S= A*square(tv[0]) + B*square(tv[1]) + C*square(tv[2]);
Real T=2*(A*tu[0]*tv[0] + B*tu[1]*tv[1] + C*tu[2]*tv[2]);
// T will be zero for spheres (A=B=C) and for various "clean" points
// on the ellipsoid where tu[i]*tv[i]==0, i=0,1,2. In that case we
// already have the ellipse we're looking for with R,S.
// R==S means curvature is the same in every direction (that's called
// an "umbilic" point). In that case tu and tv are good directions.
// I *believe* R==S -> T==0 but I don't have a proof.
Real kmax2, kmin2; // squared curvatures of ellipse
UnitVec3 dmax;
if (std::abs(R-S) < SignificantReal*std::max(R,S)) {
kmax2 = kmin2 = (R+S)/2;
dmax = tu;
} else if (std::abs(T) < SignificantReal) {
if (R < S) kmax2=S, dmax=tv, kmin2=R;
else kmax2=R, dmax=tu, kmin2=S;
} else { // T,R-S both nonzero
Real tan2t = T/(R-S); // ~20 flops
Real cos2t = 1/std::sqrt(1 + square(tan2t)); // ~40 flops
Real sin2t = tan2t*cos2t; // 1 flop
// 11 flops here
Real term = R*cos2t-S*cos2t+T*sin2t;
Real Rp = (R+S + term)/2;
Real Sp = (R+S - term)/2;
// Sort into kmax, kmin; at most one normalization done below
if (Rp < Sp) {
kmax2=Sp, kmin2=Rp;
dmax = UnitVec3((1+cos2t)*tv - sin2t*tu); // Sdir, must normalize, ~50 flops
} else {
kmax2=Rp,kmin2=Sp;
dmax = UnitVec3((1+cos2t)*tu + sin2t*tv); // Rdir, must normalize, ~50 flops
}
}
Real d = ~Q * nn; // distance along normal from center to point on ellipsoid (5 flops)
Real kmax = d * kmax2, kmin = d * kmin2; // surface curvatures (2 flops)
X_EP.updP() = Q; // the origin point
Rotation& R_EP = X_EP.updR();
// 9 flops
UnitVec3 dmin = UnitVec3(nn % dmax, true); // y=z%x ensures right handedness (already unit vector too)
R_EP.setRotationFromUnitVecsTrustMe(dmax, dmin, nn);
k = Vec2(kmax, kmin);
}
/*
* Shortcut to transform [vector] by [matrix]. This transformation occurs in world space.
*/
void SceneObjectTransform::TransformVector(Vector3& vector) const {
Transform full;
GetInheritedTransform(full, false);
full.TransformBy(this);
full.TransformVector(vector);
}
void ContactGeometry::Ellipsoid::Impl::
calcCurvature(const Vec3& point, Vec2& curvature, Rotation& orientation) const {
Transform transform;
findParaboloidAtPoint(point, transform, curvature);
orientation = transform.R();
}
/*
* Shortcut to transform [point] by [matrix]. This transformation occurs in world space.
*/
void SceneObjectTransform::TransformPoint(Point3& point) const {
Transform full;
GetInheritedTransform(full, false);
full.TransformBy(this);
full.TransformPoint(point);
}
/*!
Setup of the view port and projection initialization. In projection initialization the Ortho or Perspective is set
as per requirement.
\param[in] width of the screen.
\param[in] height of the screen.
\return void.
*/
void Renderer::setUpProjection()
{
ProgramManager* ProgramManagerObj = &RenderMemData.ProgramManagerObj;
Transform* TransformObj = &RenderMemData.TransformObj;
bool considerAspectRatio = true;
float span = 10.0;
//Transform Init() function is moved to the constructor no need to call every frame.
//TransformObj->TransformInit();
TransformObj->TransformSetMatrixMode( PROJECTION_MATRIX );
TransformObj->TransformLoadIdentity();
if ( considerAspectRatio ){
GLfloat aspectRatio = (GLfloat)RenderMemData.screenWidth / (GLfloat)RenderMemData.screenHeight;
if ( RenderMemData.isPerspective ){
TransformObj->TransformSetPerspective(60.0f, aspectRatio, 0.01, 1000, 0);
}else{
if ( RenderMemData.screenWidth <= RenderMemData.screenHeight ){
TransformObj->TransformOrtho( -span, span, -span / aspectRatio, span / aspectRatio, -span, span);
}
else{
TransformObj->TransformOrtho( -span * aspectRatio, span * aspectRatio, -span, span, -span, span);
}
}
}
else{
if ( RenderMemData.isPerspective ){
TransformObj->TransformSetPerspective(60.0f, 1, 1.0, 100, 0);
}
else{
TransformObj->TransformOrtho( -span, span, -span, span, -span, span );
}
}
TransformObj->TransformSetMatrixMode( VIEW_MATRIX );
// TransformObj->TransformLoadIdentity();
//TransformObj->TransformTranslate(0,0,-3);
static float frame = 0.0;
UpdateCamera((clock()-globalClock)/100000000);
glm::mat4* m_viewMatrix = TransformObj->TransformGetViewMatrix();
LOGI("00: %f",(*m_viewMatrix)[0][0]);
LOGI("10: %f",(*m_viewMatrix)[1][0]);
LOGI("20: %f",(*m_viewMatrix)[2][0]);
LOGI("30: %f",(*m_viewMatrix)[3][0]);
LOGI("01: %f",(*m_viewMatrix)[0][1]);
LOGI("11: %f",(*m_viewMatrix)[1][1]);
LOGI("21: %f",(*m_viewMatrix)[2][1]);
LOGI("31: %f",(*m_viewMatrix)[3][1]);
LOGI("02: %f",(*m_viewMatrix)[0][2]);
LOGI("12: %f",(*m_viewMatrix)[1][2]);
LOGI("22: %f",(*m_viewMatrix)[2][2]);
LOGI("32: %f",(*m_viewMatrix)[3][2]);
LOGI("03: %f",(*m_viewMatrix)[0][3]);
LOGI("13: %f",(*m_viewMatrix)[1][3]);
LOGI("23: %f",(*m_viewMatrix)[2][3]);
LOGI("33: %f",(*m_viewMatrix)[3][3]);
TransformObj->TransformSetMatrixMode( MODEL_MATRIX );
TransformObj->TransformLoadIdentity();
}
template<typename Scalar,int Size> void homogeneous(void)
{
/* this test covers the following files:
Homogeneous.h
*/
typedef Matrix<Scalar,Size,Size> MatrixType;
typedef Matrix<Scalar,Size,1, ColMajor> VectorType;
typedef Matrix<Scalar,Size+1,Size> HMatrixType;
typedef Matrix<Scalar,Size+1,1> HVectorType;
typedef Matrix<Scalar,Size,Size+1> T1MatrixType;
typedef Matrix<Scalar,Size+1,Size+1> T2MatrixType;
typedef Matrix<Scalar,Size+1,Size> T3MatrixType;
VectorType v0 = VectorType::Random(),
ones = VectorType::Ones();
HVectorType hv0 = HVectorType::Random();
MatrixType m0 = MatrixType::Random();
HMatrixType hm0 = HMatrixType::Random();
hv0 << v0, 1;
VERIFY_IS_APPROX(v0.homogeneous(), hv0);
VERIFY_IS_APPROX(v0, hv0.hnormalized());
VERIFY_IS_APPROX(v0.homogeneous().sum(), hv0.sum());
VERIFY_IS_APPROX(v0.homogeneous().minCoeff(), hv0.minCoeff());
VERIFY_IS_APPROX(v0.homogeneous().maxCoeff(), hv0.maxCoeff());
hm0 << m0, ones.transpose();
VERIFY_IS_APPROX(m0.colwise().homogeneous(), hm0);
VERIFY_IS_APPROX(m0, hm0.colwise().hnormalized());
hm0.row(Size-1).setRandom();
for(int j=0; j<Size; ++j)
m0.col(j) = hm0.col(j).head(Size) / hm0(Size,j);
VERIFY_IS_APPROX(m0, hm0.colwise().hnormalized());
T1MatrixType t1 = T1MatrixType::Random();
VERIFY_IS_APPROX(t1 * (v0.homogeneous().eval()), t1 * v0.homogeneous());
VERIFY_IS_APPROX(t1 * (m0.colwise().homogeneous().eval()), t1 * m0.colwise().homogeneous());
T2MatrixType t2 = T2MatrixType::Random();
VERIFY_IS_APPROX(t2 * (v0.homogeneous().eval()), t2 * v0.homogeneous());
VERIFY_IS_APPROX(t2 * (m0.colwise().homogeneous().eval()), t2 * m0.colwise().homogeneous());
VERIFY_IS_APPROX(t2 * (v0.homogeneous().asDiagonal()), t2 * hv0.asDiagonal());
VERIFY_IS_APPROX((v0.homogeneous().asDiagonal()) * t2, hv0.asDiagonal() * t2);
VERIFY_IS_APPROX((v0.transpose().rowwise().homogeneous().eval()) * t2,
v0.transpose().rowwise().homogeneous() * t2);
VERIFY_IS_APPROX((m0.transpose().rowwise().homogeneous().eval()) * t2,
m0.transpose().rowwise().homogeneous() * t2);
T3MatrixType t3 = T3MatrixType::Random();
VERIFY_IS_APPROX((v0.transpose().rowwise().homogeneous().eval()) * t3,
v0.transpose().rowwise().homogeneous() * t3);
VERIFY_IS_APPROX((m0.transpose().rowwise().homogeneous().eval()) * t3,
m0.transpose().rowwise().homogeneous() * t3);
// test product with a Transform object
Transform<Scalar, Size, Affine> aff;
Transform<Scalar, Size, AffineCompact> caff;
Transform<Scalar, Size, Projective> proj;
Matrix<Scalar, Size, Dynamic> pts;
Matrix<Scalar, Size+1, Dynamic> pts1, pts2;
aff.affine().setRandom();
proj = caff = aff;
pts.setRandom(Size,internal::random<int>(1,20));
pts1 = pts.colwise().homogeneous();
VERIFY_IS_APPROX(aff * pts.colwise().homogeneous(), (aff * pts1).colwise().hnormalized());
VERIFY_IS_APPROX(caff * pts.colwise().homogeneous(), (caff * pts1).colwise().hnormalized());
VERIFY_IS_APPROX(proj * pts.colwise().homogeneous(), (proj * pts1));
VERIFY_IS_APPROX((aff * pts1).colwise().hnormalized(), aff * pts);
VERIFY_IS_APPROX((caff * pts1).colwise().hnormalized(), caff * pts);
pts2 = pts1;
pts2.row(Size).setRandom();
VERIFY_IS_APPROX((aff * pts2).colwise().hnormalized(), aff * pts2.colwise().hnormalized());
VERIFY_IS_APPROX((caff * pts2).colwise().hnormalized(), caff * pts2.colwise().hnormalized());
VERIFY_IS_APPROX((proj * pts2).colwise().hnormalized(), (proj * pts2.colwise().hnormalized().colwise().homogeneous()).colwise().hnormalized());
// Test combination of homogeneous
VERIFY_IS_APPROX( (t2 * v0.homogeneous()).hnormalized(),
(t2.template topLeftCorner<Size,Size>() * v0 + t2.template topRightCorner<Size,1>())
/ ((t2.template bottomLeftCorner<1,Size>()*v0).value() + t2(Size,Size)) );
VERIFY_IS_APPROX( (t2 * pts.colwise().homogeneous()).colwise().hnormalized(),
(Matrix<Scalar, Size+1, Dynamic>(t2 * pts1).colwise().hnormalized()) );
VERIFY_IS_APPROX( (t2 .lazyProduct( v0.homogeneous() )).hnormalized(), (t2 * v0.homogeneous()).hnormalized() );
VERIFY_IS_APPROX( (t2 .lazyProduct ( pts.colwise().homogeneous() )).colwise().hnormalized(), (t2 * pts1).colwise().hnormalized() );
VERIFY_IS_APPROX( (v0.transpose().homogeneous() .lazyProduct( t2 )).hnormalized(), (v0.transpose().homogeneous()*t2).hnormalized() );
VERIFY_IS_APPROX( (pts.transpose().rowwise().homogeneous() .lazyProduct( t2 )).rowwise().hnormalized(), (pts1.transpose()*t2).rowwise().hnormalized() );
VERIFY_IS_APPROX( (t2.template triangularView<Lower>() * v0.homogeneous()).eval(), (t2.template triangularView<Lower>()*hv0) );
}
void projectPoint( Point& p )
{
p = transform.projectPoint(p);
}
void Locomotive::display(const Transform &viewProjectionTransform,
Transform worldTransform)
{
//
// Copy your previous (PA08) setting of `modelTransform` here.
//
if (path == NULL)
{
Transform identity;
modelTransform = identity;
}
else
{
// Perform in reverse order (c -> b -> a)
modelTransform = path->coordinateFrame(u); // "Set?"
modelTransform.scale(0.15, 0.075, 0.15); // Scale
modelTransform.translate(0.0, 0.25, 0.0); // Translate
}
if (eadsShaderProgram) { // will be NULL in the template
Rgb firebrick((178.0/255.0), (34.0/255.0), (34.0/255.0));
double specFrac = 0.25; // fraction of reflected power that's specular
Rgb ambDiffBaseRgb = (1.0 - specFrac) * firebrick;
//Rgb ambDiffBaseRgb = (1.0 - specFrac) * baseRgb;
Transform identityTransform;
eadsShaderProgram->setEmittance(blackColor);
eadsShaderProgram->setAmbient(0.2 * ambDiffBaseRgb);
eadsShaderProgram->setDiffuse(0.8 * ambDiffBaseRgb);
eadsShaderProgram->setSpecular(Rgb(specFrac, specFrac, specFrac),
10.0);
worldTransform *= modelTransform;
eadsShaderProgram->setModelViewProjectionMatrix(
viewProjectionTransform * worldTransform);
eadsShaderProgram->setWorldMatrix(worldTransform);
eadsShaderProgram->setNormalMatrix(
worldTransform.getNormalTransform());
eadsShaderProgram->start();
}
const double quillLength = 0.04;
engine->draw();
engine->displayHedgehogIfRequested(viewProjectionTransform,
worldTransform,
quillLength);
cab->draw();
cab->displayHedgehogIfRequested(viewProjectionTransform,
worldTransform,
quillLength);
smokeStack->draw();
smokeStack->displayHedgehogIfRequested(viewProjectionTransform,
worldTransform,
quillLength);
if (controller.axesEnabled)
coordinateAxes->display(viewProjectionTransform, worldTransform);
}
void Enemy::init(Transform* player, glm::vec3 pos, EnemyType type) {
this->player = player;
Transform* model = registerModel();
transform->parentAll(model);
glm::vec3 scale = glm::vec3(1.0f, 2.0f, 1.0f);
glm::vec3 variance = Mth::randInsideUnitCube();
variance.x = variance.z = abs(variance.x);
scale += variance * .25f;
glm::vec3 color;
switch (type) {
case EnemyType::ELITE: {
scale *= Mth::rand01() + 2.0f;
color = glm::vec3(0.8f, 1.0f, 0.6f);
speed = Mth::rand01() * 5.0f + 10.0f;
jumpVel = Mth::rand01() * 10.0f + 30.0f;
health = 100.0f;
Transform* larm = registerModel();
Transform* rarm = registerModel();
larm->setPos(-scale.x * 0.75f, scale.y * 0.75f, 0.0f);
rarm->setPos(scale.x * 0.75f, scale.y * 0.75f, 0.0f);
larm->setScale(scale*0.75f);
rarm->setScale(scale*0.75f);
transform->parentAllWithColor(larm, rarm);
break; }
case EnemyType::BASIC: {
color = glm::vec3(1.0f, Mth::rand01() * 0.3f, Mth::rand01() * 0.3f);
speed = Mth::rand01() * 5.0f + 5.0f;
jumpVel = Mth::rand01() * 10.0f + 20.0f;
health = 20.0f;
//scale = glm::vec3(0.75f, 2.0f, 0.75f);
Transform* head = registerModel();
head->setPos(0.0f, scale.y * 0.75f, 0.0f);
head->setScale(1.3f, 0.7f, 1.3f);
head->color = glm::vec3(1.0f, 0.0f, 0.0f);
transform->parentAll(head);
break; }
default:
break;
}
transform->setPos(pos);
model->setPos(0.0f, scale.y / 2.0f, 0.0f);
model->setScale(scale);
model->color = color;
glm::vec3 min = glm::vec3(-0.5f, 0.0f, -0.5f)*scale;
glm::vec3 max = glm::vec3(0.5f, 1.0f, 0.5f)*scale;
collider->setExtents(min, max);
}
/**
* Send the snapshot to the appropriate server.
*
* This function will send messages to the appropriate gameServer to
* create a proxy for the objects in the snapshot, then it will send a
* message to central to relinquish authority for the objects.
*
* @return true if the objects were sent, false if they were not.
*/
bool SwgSnapshot::send(GameServerConnection *connection) const
{
PROFILER_AUTO_BLOCK_DEFINE("SwgSnapshot::send");
if (connection==0)
return false;
DEBUG_REPORT_LOG(ConfigServerDatabase::getLogObjectLoading(),("Sending Snapshot\n"));
PROFILER_BLOCK_DEFINE(prebaselinesBlock,"sendPreBaselinesCustomData");
PROFILER_BLOCK_ENTER(prebaselinesBlock);
for (LocatorListType::const_iterator loc=m_locatorList.begin(); loc!=m_locatorList.end(); ++loc)
{
(*loc)->sendPreBaselinesCustomData(*connection);
}
PROFILER_BLOCK_LEAVE(prebaselinesBlock);
OIDListType oidList;
m_objectTableBuffer.getObjectList(oidList);
PROFILER_BLOCK_DEFINE(sendObjectData,"send object data");
PROFILER_BLOCK_ENTER(sendObjectData);
static std::vector<BatchBaselinesMessageData> baselines;
baselines.clear();
for (OIDListType::iterator i=oidList.begin(); i!=oidList.end(); ++i)
{
PROFILER_BLOCK_DEFINE(createBlock, "object create and position");
PROFILER_BLOCK_ENTER(createBlock);
const DBSchema::ObjectBufferRow *baseData=m_objectTableBuffer.findConstRowByIndex((*i));
NOT_NULL(baseData);
NetworkId networkId=(*i);
DEBUG_FATAL(networkId != baseData->object_id.getValue(),("Object ID and row value didn't match"));
uint32 crc = baseData->object_template_id.getValue();
CreateObjectByCrcMessage com(networkId,
crc,
static_cast<unsigned short>(baseData->type_id.getValue()),
true,
NetworkId(baseData->contained_by.getValue()));
// connection->send(com,true);
DEBUG_REPORT_LOG(ConfigServerDatabase::getLogObjectLoading(),("\tSent CreateObjectMessage for object %s\n",networkId.getValueString().c_str()));
Transform t;
Quaternion q(static_cast<real>(baseData->quaternion_w.getValue()),
static_cast<real>(baseData->quaternion_x.getValue()),
static_cast<real>(baseData->quaternion_y.getValue()),
static_cast<real>(baseData->quaternion_z.getValue()));
q.getTransform(&t); // reorients the transform according to the quaternion
t.validate();
t.setPosition_p(Vector (static_cast<real>(baseData->x.getValue()),
static_cast<real>(baseData->y.getValue()),
static_cast<real>(baseData->z.getValue())));
UpdateObjectPositionMessage uopm(
NetworkId(baseData->object_id.getValue()),
t,
t,
NetworkId(baseData->contained_by.getValue()),
baseData->slot_arrangement.getValue(),
NetworkId(baseData->load_with.getValue()),
baseData->player_controlled.getValue(),
false);
// connection->send(uopm,true);
PROFILER_BLOCK_LEAVE(createBlock);
PROFILER_BLOCK_DEFINE(encodeData,"encode data");
PROFILER_BLOCK_ENTER(encodeData);
bool okToSend = encodeParentClientData(networkId,baseData->type_id.getValue(),baselines);
okToSend = okToSend && encodeClientData(networkId,baseData->type_id.getValue(),baselines);
okToSend = okToSend && encodeServerData(networkId,baseData->type_id.getValue(),baselines);
okToSend = okToSend && encodeSharedData(networkId,baseData->type_id.getValue(),baselines);
PROFILER_BLOCK_LEAVE(encodeData);
if (okToSend)
{
PROFILER_AUTO_BLOCK_DEFINE("connection->send (baselines)");
connection->send(com,true);
connection->send(uopm,true);
}
else
{
(*i)=NetworkId::cms_invalid;
}
}
BatchBaselinesMessage bbm(baselines);
connection->send(bbm,true);
PROFILER_BLOCK_LEAVE(sendObjectData);
PROFILER_BLOCK_DEFINE(sendEndBaselines,"send EndBaselines");
PROFILER_BLOCK_ENTER(sendEndBaselines);
// Send EndBaselines in reverse order (container & portal system requires this)
for (OIDListType::reverse_iterator r=oidList.rbegin(); r!=oidList.rend(); ++r)
{
if (*r != NetworkId::cms_invalid)
{
EndBaselinesMessage const ebm(*r);
connection->send(ebm,true);
}
// DEBUG_REPORT_LOG(true, ("\tSent EndBaselinesMessage for object %i\n",(*r).getValue()));
}
PROFILER_BLOCK_LEAVE(sendEndBaselines);
// Send resource data (if any) after all objects but before the post-baselines custom data
m_resourceTypeBuffer.sendResourceTypeObjects(*connection);
m_bountyHunterTargetBuffer.sendBountyHunterTargetMessage (*connection);
// Send post-baselines custom data, such as "UniverseComplete" messages, etc.
PROFILER_BLOCK_DEFINE(sendPostBaselinesCustomData,"sendPostBaselinesCustomData");
PROFILER_BLOCK_ENTER(sendPostBaselinesCustomData);
for (LocatorListType::const_iterator loc2=m_locatorList.begin(); loc2!=m_locatorList.end(); ++loc2)
{
(*loc2)->sendPostBaselinesCustomData(*connection);
}
PROFILER_BLOCK_LEAVE(sendPostBaselinesCustomData);
// Send any MessageTos for these objects
// This must happen after EndBaselinesMessage is sent for all of the objects
{
PROFILER_AUTO_BLOCK_DEFINE("send MessageTos");
m_messageBuffer.sendMessages(*connection);
for (OIDListType::const_iterator i=oidList.begin(); i!=oidList.end(); ++i)
{
if (*i != NetworkId::cms_invalid)
MessageToManager::getInstance().sendMessagesForObject(*i, *connection);
}
}
DEBUG_REPORT_LOG(ConfigServerDatabase::getLogObjectLoading(),("Done sending Snapshot\n"));
return true;
}
//----------------------------------------------------------------------------
Transform Transform::operator* (const Transform& transform) const
{
if (IsIdentity())
{
return transform;
}
if (transform.IsIdentity())
{
return *this;
}
Transform product;
if (mIsRSMatrix && transform.mIsRSMatrix)
{
if (mIsUniformScale)
{
product.SetRotate(mMatrix*transform.mMatrix);
product.SetTranslate(GetUniformScale()*(
mMatrix*transform.mTranslate) + mTranslate);
if (transform.IsUniformScale())
{
product.SetUniformScale(
GetUniformScale()*transform.GetUniformScale());
}
else
{
product.SetScale(GetUniformScale()*transform.GetScale());
}
return product;
}
}
// In all remaining cases, the matrix cannot be written as R*S*X+T.
HMatrix matMA = (mIsRSMatrix ? mMatrix.TimesDiagonal(mScale) : mMatrix);
HMatrix matMB = (transform.mIsRSMatrix ?
transform.mMatrix.TimesDiagonal(transform.mScale) :
transform.mMatrix);
product.SetMatrix(matMA*matMB);
product.SetTranslate(matMA*transform.mTranslate + mTranslate);
return product;
}
void DotaAnimParser::setFrameTransform(AnimationData *animationData, const ArmatureData *armatureData, const BoneData *boneData, TransformFrame *frame)
{
frame->transform = frame->global;
BoneData *parentData = armatureData->getBoneData(boneData->parent);
if (parentData)
{
TransformTimeline *parentTimeline = animationData->getTimeline(parentData->name);
if (parentTimeline)
{
std::vector<TransformTimeline*> parentTimelineList;
std::vector<BoneData*> parentDataList;
while (parentTimeline)
{
parentTimelineList.push_back(parentTimeline);
parentDataList.push_back(parentData);
parentData = armatureData->getBoneData(parentData->parent);
if (parentData)
{
parentTimeline = animationData->getTimeline(parentData->name);
}
else
{
parentTimeline = nullptr;
}
}
Matrix helpMatrix;
Transform currentTransform;
Transform *globalTransform = nullptr;
for (size_t i = parentTimelineList.size(); i--;)
{
parentTimeline = parentTimelineList[i];
parentData = parentDataList[i];
getTimelineTransform(parentTimeline, frame->position, ¤tTransform, !globalTransform);
if (globalTransform)
{
//if(inheritRotation)
//{
globalTransform->skewX += currentTransform.skewX + parentTimeline->originTransform.skewX + parentData->transform.skewX;
globalTransform->skewY += currentTransform.skewY + parentTimeline->originTransform.skewY + parentData->transform.skewY;
//}
//if(inheritScale)
//{
// globalTransform.scaleX *= currentTransform.scaleX + parentTimeline.originTransform.scaleX;
// globalTransform.scaleY *= currentTransform.scaleY + parentTimeline.originTransform.scaleY;
//}
//else
//{
globalTransform->scaleX = currentTransform.scaleX + parentTimeline->originTransform.scaleX + parentData->transform.scaleX;
globalTransform->scaleY = currentTransform.scaleY + parentTimeline->originTransform.scaleY + parentData->transform.scaleY;
//}
const float x = currentTransform.x + parentTimeline->originTransform.x + parentData->transform.x;
const float y = currentTransform.y + parentTimeline->originTransform.y + parentData->transform.y;
globalTransform->x = helpMatrix.a * x + helpMatrix.c * y + helpMatrix.tx;
globalTransform->y = helpMatrix.d * y + helpMatrix.b * x + helpMatrix.ty;
}
else
{
globalTransform = new Transform();
*globalTransform = currentTransform;
}
globalTransform->toMatrix(helpMatrix, true);
}
frame->transform.transformWith(*globalTransform);
if (globalTransform)
{
delete globalTransform;
globalTransform = nullptr;
}
}
}
}
void CameraComponent::SetRotation(const Vector3 & rot)
{
Transform* tr = static_cast<Transform*>(GetSibling(CT_Transform));
tr->SetRotation(rot);
//rotation_ = rot;
}
virtual void init() {
print_line("INITIALIZING TEST RENDER");
VisualServer *vs=VisualServer::get_singleton();
test_cube = vs->get_test_cube();
scenario = vs->scenario_create();
Vector<Vector3> vts;
/*
DVector<Plane> sp = Geometry::build_sphere_planes(2,5,5);
Geometry::MeshData md2 = Geometry::build_convex_mesh(sp);
vts=md2.vertices;
*/
/*
static const int s = 20;
for(int i=0;i<s;i++) {
Matrix3 rot(Vector3(0,1,0),i*Math_PI/s);
for(int j=0;j<s;j++) {
Vector3 v;
v.x=Math::sin(j*Math_PI*2/s);
v.y=Math::cos(j*Math_PI*2/s);
vts.push_back( rot.xform(v*2 ) );
}
}*/
/*for(int i=0;i<100;i++) {
vts.push_back( Vector3(Math::randf()*2-1.0,Math::randf()*2-1.0,Math::randf()*2-1.0).normalized()*2);
}*/
/*
vts.push_back(Vector3(0,0,1));
vts.push_back(Vector3(0,0,-1));
vts.push_back(Vector3(0,1,0));
vts.push_back(Vector3(0,-1,0));
vts.push_back(Vector3(1,0,0));
vts.push_back(Vector3(-1,0,0));*/
vts.push_back(Vector3(1,1,1));
vts.push_back(Vector3(1,-1,1));
vts.push_back(Vector3(-1,1,1));
vts.push_back(Vector3(-1,-1,1));
vts.push_back(Vector3(1,1,-1));
vts.push_back(Vector3(1,-1,-1));
vts.push_back(Vector3(-1,1,-1));
vts.push_back(Vector3(-1,-1,-1));
Geometry::MeshData md;
Error err = QuickHull::build(vts,md);
print_line("ERR: "+itos(err));
test_cube = vs->mesh_create();
vs->mesh_add_surface_from_mesh_data(test_cube,md);
//vs->scenario_set_debug(scenario,VS::SCENARIO_DEBUG_WIREFRAME);
/*
RID sm = vs->shader_create();
//vs->shader_set_fragment_code(sm,"OUT_ALPHA=mod(TIME,1);");
//vs->shader_set_vertex_code(sm,"OUT_VERTEX=IN_VERTEX*mod(TIME,1);");
vs->shader_set_fragment_code(sm,"OUT_DIFFUSE=vec3(1,0,1);OUT_GLOW=abs(sin(TIME));");
RID tcmat = vs->mesh_surface_get_material(test_cube,0);
vs->material_set_shader(tcmat,sm);
*/
List<String> cmdline = OS::get_singleton()->get_cmdline_args();
int object_count = OBJECT_COUNT;
if (cmdline.size() > 0 && cmdline[cmdline.size()-1].to_int()) {
object_count = cmdline[cmdline.size()-1].to_int();
};
for (int i=0;i<object_count;i++) {
InstanceInfo ii;
ii.instance = vs->instance_create2( test_cube, scenario );
ii.base.translate( Math::random(-20,20), Math::random(-20,20),Math::random(-20,18) );
ii.base.rotate( Vector3(0,1,0), Math::randf() * Math_PI );
ii.base.rotate( Vector3(1,0,0), Math::randf() * Math_PI );
vs->instance_set_transform( ii.instance, ii.base );
ii.rot_axis = Vector3( Math::random(-1,1), Math::random(-1,1), Math::random(-1,1) ).normalized();
instances.push_back(ii);
}
camera = vs->camera_create();
// vs->camera_set_perspective( camera, 60.0,0.1, 100.0 );
viewport = vs->viewport_create();
vs->viewport_attach_to_screen(viewport);
vs->viewport_attach_camera( viewport, camera );
vs->viewport_set_scenario( viewport, scenario );
vs->camera_set_transform(camera, Transform( Matrix3(), Vector3(0,3,30 ) ) );
vs->camera_set_perspective( camera, 60, 0.1, 1000);
/*
RID lightaux = vs->light_create( VisualServer::LIGHT_OMNI );
vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_RADIUS, 80 );
vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_ATTENUATION, 1 );
vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_ENERGY, 1.5 );
light = vs->instance_create( lightaux );
*/
RID lightaux;
//*
lightaux = vs->light_create( VisualServer::LIGHT_DIRECTIONAL );
vs->light_set_color( lightaux, VisualServer::LIGHT_COLOR_AMBIENT, Color(0.0,0.0,0.0) );
vs->light_set_color( lightaux, VisualServer::LIGHT_COLOR_DIFFUSE, Color(1.0,1.0,1.0) );
//vs->light_set_shadow( lightaux, true );
light = vs->instance_create2( lightaux, scenario );
Transform lla;
//lla.set_look_at(Vector3(),Vector3(1,-1,1),Vector3(0,1,0));
lla.set_look_at(Vector3(),Vector3(-0.000000,-0.836026,-0.548690),Vector3(0,1,0));
vs->instance_set_transform( light, lla );
// */
//*
lightaux = vs->light_create( VisualServer::LIGHT_OMNI );
vs->light_set_color( lightaux, VisualServer::LIGHT_COLOR_AMBIENT, Color(0.0,0.0,1.0) );
vs->light_set_color( lightaux, VisualServer::LIGHT_COLOR_DIFFUSE, Color(1.0,1.0,0.0) );
vs->light_set_param( lightaux, VisualServer::LIGHT_PARAM_RADIUS, 4 );
vs->light_set_param( lightaux, VisualServer::LIGHT_PARAM_ENERGY, 8 );
//vs->light_set_shadow( lightaux, true );
//light = vs->instance_create( lightaux );
// */
ofs=0;
quit=false;
}
void callback(
const sensor_msgs::ImageConstPtr& imageRectLeft,
const sensor_msgs::ImageConstPtr& imageRectRight,
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
if(!this->isPaused())
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)");
return;
}
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
ros::WallTime time = ros::WallTime::now();
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
if(model.baseline() <= 0)
{
ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", model.baseline());
return;
}
rtabmap::StereoCameraModel stereoModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
else
{
ROS_WARN("Odom: input images empty?!?");
}
}
}
bool PathEditorPlugin::forward_spatial_input_event(Camera* p_camera,const InputEvent& p_event) {
if (!path)
return false;
Ref<Curve3D> c=path->get_curve();
if (c.is_null())
return false;
Transform gt = path->get_global_transform();
Transform it = gt.affine_inverse();
static const int click_dist = 10; //should make global
if (p_event.type==InputEvent::MOUSE_BUTTON) {
const InputEventMouseButton &mb=p_event.mouse_button;
Point2 mbpos(mb.x,mb.y);
if (mb.pressed && mb.button_index==BUTTON_LEFT && (curve_create->is_pressed() || (curve_edit->is_pressed() && mb.mod.control))) {
//click into curve, break it down
Vector3Array v3a = c->tesselate();
int idx=0;
int rc=v3a.size();
int closest_seg=-1;
Vector3 closest_seg_point;
float closest_d=1e20;
if (rc>=2) {
Vector3Array::Read r = v3a.read();
if (p_camera->unproject_position(gt.xform(c->get_point_pos(0))).distance_to(mbpos)<click_dist)
return false; //nope, existing
for(int i=0;i<c->get_point_count()-1;i++) {
//find the offset and point index of the place to break up
int j=idx;
if (p_camera->unproject_position(gt.xform(c->get_point_pos(i+1))).distance_to(mbpos)<click_dist)
return false; //nope, existing
while(j<rc && c->get_point_pos(i+1)!=r[j]) {
Vector3 from =r[j];
Vector3 to =r[j+1];
real_t cdist = from.distance_to(to);
from=gt.xform(from);
to=gt.xform(to);
if (cdist>0) {
Vector2 s[2];
s[0] = p_camera->unproject_position(from);
s[1] = p_camera->unproject_position(to);
Vector2 inters = Geometry::get_closest_point_to_segment_2d(mbpos,s);
float d = inters.distance_to(mbpos);
if (d<10 && d<closest_d) {
closest_d=d;
closest_seg=i;
Vector3 ray_from=p_camera->project_ray_origin(mbpos);
Vector3 ray_dir=p_camera->project_ray_normal(mbpos);
Vector3 ra,rb;
Geometry::get_closest_points_between_segments(ray_from,ray_from+ray_dir*4096,from,to,ra,rb);
closest_seg_point=it.xform(rb);
}
}
j++;
}
if (idx==j)
idx++; //force next
else
idx=j; //swap
if (j==rc)
break;
}
}
UndoRedo *ur = editor->get_undo_redo();
if (closest_seg!=-1) {
//subdivide
ur->create_action("Split Path");
ur->add_do_method(c.ptr(),"add_point",closest_seg_point,Vector3(),Vector3(),closest_seg+1);
ur->add_undo_method(c.ptr(),"remove_point",closest_seg+1);
ur->commit_action();;
return true;
} else {
Vector3 org;
if (c->get_point_count()==0)
org=path->get_transform().get_origin();
else
org=gt.xform(c->get_point_pos(c->get_point_count()));
Plane p(org,p_camera->get_transform().basis.get_axis(2));
Vector3 ray_from=p_camera->project_ray_origin(mbpos);
Vector3 ray_dir=p_camera->project_ray_normal(mbpos);
Vector3 inters;
if (p.intersects_ray(ray_from,ray_dir,&inters)) {
ur->create_action("Add Point to Curve");
ur->add_do_method(c.ptr(),"add_point",it.xform(inters),Vector3(),Vector3(),-1);
ur->add_undo_method(c.ptr(),"remove_point",c->get_point_count());
ur->commit_action();;
return true;
}
//add new at pos
}
} else if (mb.pressed && ((mb.button_index==BUTTON_LEFT && curve_del->is_pressed()) || (mb.button_index==BUTTON_RIGHT && curve_edit->is_pressed()))) {
int erase_idx=-1;
for(int i=0;i<c->get_point_count();i++) {
//find the offset and point index of the place to break up
if (p_camera->unproject_position(gt.xform(c->get_point_pos(i))).distance_to(mbpos)<click_dist) {
erase_idx=i;
break;
}
}
if (erase_idx!=-1) {
UndoRedo *ur = editor->get_undo_redo();
ur->create_action("Remove Path Point");
ur->add_do_method(c.ptr(),"remove_point",erase_idx);
ur->add_undo_method(c.ptr(),"add_point",c->get_point_pos(erase_idx),c->get_point_in(erase_idx),c->get_point_out(erase_idx),erase_idx);
ur->commit_action();
return true;
}
}
}
return false;
}