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; }