TEST(QuaternionTest, Operators) { Quaternion q; q = Quaternion(1, 2, 3, 4) + Quaternion(5, 6, 7, 8); EXPECT_NEAR(6, q.x(), 1e-100); EXPECT_NEAR(8, q.y(), 1e-100); EXPECT_NEAR(10, q.z(), 1e-100); EXPECT_NEAR(12, q.w(), 1e-100); q = Quaternion(8, 7, 6, 5) - Quaternion(1, 2, 3, 4); EXPECT_NEAR(7, q.x(), 1e-100); EXPECT_NEAR(5, q.y(), 1e-100); EXPECT_NEAR(3, q.z(), 1e-100); EXPECT_NEAR(1, q.w(), 1e-100); q = Quaternion(1, 2, 3, 4) * real_t(2); EXPECT_NEAR(2, q.x(), 1e-100); EXPECT_NEAR(4, q.y(), 1e-100); EXPECT_NEAR(6, q.z(), 1e-100); EXPECT_NEAR(8, q.w(), 1e-100); q = real_t(2) * Quaternion(4, 3, 2, 1); EXPECT_NEAR(8, q.x(), 1e-100); EXPECT_NEAR(6, q.y(), 1e-100); EXPECT_NEAR(4, q.z(), 1e-100); EXPECT_NEAR(2, q.w(), 1e-100); }
VehicleWheel::VehicleWheel() { steers=false; engine_traction=false; m_steering = real_t(0.); //m_engineForce = real_t(0.); m_rotation = real_t(0.); m_deltaRotation = real_t(0.); m_brake = real_t(0.); m_rollInfluence = real_t(0.1); m_suspensionRestLength = 0.15; m_wheelRadius = 0.5;//0.28; m_suspensionStiffness = 5.88; m_wheelsDampingCompression = 0.83; m_wheelsDampingRelaxation = 0.88; m_frictionSlip = 10.5; m_bIsFrontWheel = false; m_maxSuspensionTravelCm = 500; m_maxSuspensionForce = 6000; m_suspensionRelativeVelocity=0; m_clippedInvContactDotSuspension=1.0; m_raycastInfo.m_isInContact=false; body=NULL; }
void RaytracerApplication::toggle_raytracing( int width, int height ) { assert( width > 0 && height > 0 ); if ( !raytracing ) { if ( buf_width != width || buf_height != height ) { free( buffer ); buffer = (unsigned char*) malloc( BUFFER_SIZE( width, height ) ); if ( !buffer ) { std::cout << "Unable to allocate buffer.\n"; return; } buf_width = width; buf_height = height; } scene.camera.aspect = real_t( width ) / real_t( height ); if ( !raytracer.initialize( &scene, width, height ) ) { std::cout << "Raytracer initialization failed.\n"; return; } raytrace_finished = false; } raytracing = !raytracing; }
std::pair <Matrix, Matrix> Matrix::LUDecomposition(const Matrix &M) { ASSERT_DOMAIN(M.Square()); const size_t dim = M.Height(); Matrix L = M.LowerTriangular(), U = M.UpperTriangular(); for(size_t i = 0; i < dim; ++i) { U[i][i] = 1; } for(size_t i = 0; i < dim; ++i) { for(size_t j = i; j < dim; ++j) { for(size_t k = 0; k < i; ++k) L[j][i] -= L[j][k] * U[k][i]; } for(size_t j = i; j < dim; ++j) { real_t sum = 0; for(size_t k = 0; k < i; ++k) sum += L[i][k] * U[k][j]; if(L[i][i] == real_t(0)) { throw real_t(0); } U[i][j] = (M[i][j] - sum) / L[i][i]; } } return std::pair <Matrix, Matrix> (L, U); }
void BlitWave::GenerateWavetables() { real_t A4_freq = 440.0f; real_t two = pow(2.0f,1/12.0f); real_t f_s = real_t(sample_rate_); for (int note=0;note<128;++note) { if (wavetables[note] == nullptr) wavetables[note] = new real_t[4096]; auto freq = A4_freq * pow(two,note-69.0f); auto harmonics = uint32_t(f_s / (freq * 2.0f)); for (int n=0;n<4096;++n) { real_t result = 0.0f; real_t x = 2.0f*XM_PI*n*freq/(f_s); for (uint32_t i=1;i<=harmonics;++i) { real_t h = real_t(i); real_t m = pow(cos((i-1.0f)*XM_PI/(2.0f*harmonics)),2.0f); result += m*(1/h)*sin(h*x); } wavetables[note][n] = result; } } }
HingeJointSW::HingeJointSW(BodySW *rbA, BodySW *rbB, const Transform &frameA, const Transform &frameB) : JointSW(_arr, 2) { A = rbA; B = rbB; m_rbAFrame = frameA; m_rbBFrame = frameB; // flip axis m_rbBFrame.basis[0][2] *= real_t(-1.); m_rbBFrame.basis[1][2] *= real_t(-1.); m_rbBFrame.basis[2][2] *= real_t(-1.); //start with free m_lowerLimit = Math_PI; m_upperLimit = -Math_PI; m_useLimit = false; m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; tau = 0.3; m_angularOnly = false; m_enableAngularMotor = false; A->add_constraint(this, 0); B->add_constraint(this, 1); }
void compute_deltat(real_t *dt, const hydroparam_t H, hydrowork_t * Hw, hydrovar_t * Hv, hydrovarwork_t * Hvw) { real_t cournox, cournoy; int j, jend, slices, Hstep, Hmin, Hmax; real_t (*e)[H.nxyt]; real_t (*c)[H.nxystep]; real_t (*q)[H.nxystep][H.nxyt]; WHERE("compute_deltat"); // compute time step on grid interior cournox = zero; cournoy = zero; c = (real_t (*)[H.nxystep]) Hw->c; e = (real_t (*)[H.nxystep]) Hw->e; q = (real_t (*)[H.nxystep][H.nxyt]) Hvw->q; Hstep = H.nxystep; Hmin = H.jmin + ExtraLayer; Hmax = H.jmax - ExtraLayer; for (j = Hmin; j < Hmax; j += Hstep) { jend = j + Hstep; if (jend >= Hmax) jend = Hmax; slices = jend - j; // numbre of slices to compute ComputeQEforRow(j, H.smallr, H.nx, H.nxt, H.nyt, H.nxyt, H.nvar, slices, Hstep, Hv->uold, q, e); equation_of_state(0, H.nx, H.nxyt, H.nvar, H.smallc, H.gamma, slices, Hstep, e, q, c); courantOnXY(&cournox, &cournoy, H.nx, H.nxyt, H.nvar, slices, Hstep, c, q, Hw->tmpm1, Hw->tmpm2); // fprintf(stdout, "[%2d]\t%g %g %g %g\n", H.mype, cournox, cournoy, H.smallc, H.courant_factor); } *dt = H.courant_factor * H.dx / MAX(cournox, MAX(cournoy, H.smallc)); FLOPS(1, 1, 2, 0); // fprintf(stdout, "[%2d]\t%g %g %g %g %g %g\n", H.mype, cournox, cournoy, H.smallc, H.courant_factor, H.dx, *dt); } // compute_deltat
void RaytracerApplication::render() { int width, height; get_dimension( &width, &height ); glViewport( 0, 0, width, height ); Camera& camera = scene.camera; camera.aspect = real_t( width ) / real_t( height ); glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); glMatrixMode( GL_PROJECTION ); glLoadIdentity(); glMatrixMode( GL_MODELVIEW ); glLoadIdentity(); if ( raytracing ) { assert( buffer ); glColor4d( 1.0, 1.0, 1.0, 1.0 ); glRasterPos2f( -1.0f, -1.0f ); glDrawPixels( buf_width, buf_height, GL_RGBA, GL_UNSIGNED_BYTE, &buffer[0] ); } else { glPushAttrib( GL_ALL_ATTRIB_BITS ); render_scene( scene ); glPopAttrib(); } }
void PhysicsApplication::render() { int width, height; // query current window size, resize viewport get_dimension( &width, &height ); glViewport( 0, 0, width, height ); // fix camera aspect Camera& camera = scene.camera; camera.aspect = real_t( width ) / real_t( height ); // clear buffer glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); // reset matrices glMatrixMode( GL_PROJECTION ); glLoadIdentity(); glMatrixMode( GL_MODELVIEW ); glLoadIdentity(); glPushAttrib( GL_ALL_ATTRIB_BITS ); render_scene( scene ); glPopAttrib(); }
void SliderJointSW::testLinLimits(void) { m_solveLinLim = false; m_linPos = m_depth[0]; if(m_lowerLinLimit <= m_upperLinLimit) { if(m_depth[0] > m_upperLinLimit) { m_depth[0] -= m_upperLinLimit; m_solveLinLim = true; } else if(m_depth[0] < m_lowerLinLimit) { m_depth[0] -= m_lowerLinLimit; m_solveLinLim = true; } else { m_depth[0] = real_t(0.); } } else { m_depth[0] = real_t(0.); } } // SliderJointSW::testLinLimits()
void testHelperFunctions( const std::string & meshFile, const uint_t numTotalBlocks ) { auto mesh = make_shared<MeshType>(); mesh::readAndBroadcast( meshFile, *mesh); auto triDist = make_shared< mesh::TriangleDistance<MeshType> >( mesh ); auto distanceOctree = make_shared< DistanceOctree< MeshType > >( triDist ); const real_t meshVolume = real_c( computeVolume( *mesh ) ); const real_t blockVolume = meshVolume / real_c( numTotalBlocks ); static const real_t cellsPersBlock = real_t(1000); const real_t cellVolume = blockVolume / cellsPersBlock; const real_t dx = std::pow( cellVolume, real_t(1) / real_t(3) ); WALBERLA_LOG_INFO_ON_ROOT( "Creating SBF with createStructuredBlockStorageInsideMesh with block size" ); auto sbf0 = mesh::createStructuredBlockStorageInsideMesh( distanceOctree, dx, numTotalBlocks ); WALBERLA_LOG_INFO_ON_ROOT( "Creating SBF with createStructuredBlockStorageInsideMesh with block size" ); Vector3<uint_t> blockSize( sbf0->getNumberOfXCells(), sbf0->getNumberOfYCells(), sbf0->getNumberOfZCells() ); auto sbf1 = mesh::createStructuredBlockStorageInsideMesh( distanceOctree, dx, blockSize ); auto exteriorAabb = computeAABB( *mesh ).getScaled( real_t(2) ); WALBERLA_LOG_INFO_ON_ROOT( "Creating SBF with createStructuredBlockStorageInsideMesh with block size" ); auto sbf2 = mesh::createStructuredBlockStorageOutsideMesh( exteriorAabb, distanceOctree, dx, numTotalBlocks ); WALBERLA_LOG_INFO_ON_ROOT( "Creating SBF with createStructuredBlockStorageInsideMesh with block size" ); blockSize = Vector3<uint_t>( sbf2->getNumberOfXCells(), sbf2->getNumberOfYCells(), sbf2->getNumberOfZCells() ); auto sbf3 = mesh::createStructuredBlockStorageOutsideMesh( exteriorAabb, distanceOctree, dx, blockSize ); }
void testPointUnitSphere( const math::Vector3<real_t> & p ) { static const geometry::Sphere UNIT_SPHERE( math::Vector3<real_t>( 0, 0, 0 ), real_t( 1 ) ); static const real_t EPSILON = real_t(1e-4); real_t q = lbm::intersectionRatioBisection( UNIT_SPHERE, p, -p, EPSILON ); Vector3<real_t> intersectionPoint = p + (-p) * q; real_t intersectionRadius = intersectionPoint.length(); WALBERLA_CHECK_LESS( std::fabs( intersectionRadius - real_t( 1 ) ), EPSILON ); q = lbm::intersectionRatioSphere( UNIT_SPHERE, p, -p ); intersectionPoint = p + ( -p ) * q; intersectionRadius = intersectionPoint.length(); WALBERLA_CHECK_LESS( std::fabs( intersectionRadius - real_t( 1 ) ), EPSILON ); q = lbm::intersectionRatio( UNIT_SPHERE, p, -p, EPSILON ); intersectionPoint = p + ( -p ) * q; intersectionRadius = intersectionPoint.length(); WALBERLA_CHECK_LESS( std::fabs( intersectionRadius - real_t( 1 ) ), EPSILON ); }
ALWAYS_INLINE void operator=(float val) { #ifdef NO_INTRINSICS REAL_VEC_LOOP(i) u.r[i] = real_t(val); #else u.mr = INAME(set1)(real_t(val)); #endif }
void RaytracerApplication::toggle_raytracing( int width, int height ) { assert( width > 0 && height > 0 ); // do setup if starting a new raytrace if ( !raytracing ) { // only re-allocate if the dimensions changed if ( buf_width != width || buf_height != height ) { free( buffer ); buffer = (unsigned char*) malloc( BUFFER_SIZE( width, height ) ); if ( !buffer ) { std::cout << "Unable to allocate buffer.\n"; return; // leave untoggled since we have no buffer. } buf_width = width; buf_height = height; } // initialize the raytracer (first make sure camera aspect is correct) scene.camera.aspect = real_t( width ) / real_t( height ); if (!raytracer.initialize(&scene, options.num_samples, width, height)) { std::cout << "Raytracer initialization failed.\n"; return; // leave untoggled since initialization failed. } // reset flag that says we are done raytrace_finished = false; } raytracing = !raytracing; }
bool Tween::interpolate_property(Object *p_object , String p_property , Variant p_initial_val , Variant p_final_val , real_t p_times_in_sec , TransitionType p_trans_type , EaseType p_ease_type , real_t p_delay ) { if(pending_update != 0) { _add_pending_command("interpolate_property" , p_object , p_property , p_initial_val , p_final_val , p_times_in_sec , p_trans_type , p_ease_type , p_delay ); return true; } // convert INT to REAL is better for interpolaters if(p_initial_val.get_type() == Variant::INT) p_initial_val = p_initial_val.operator real_t(); if(p_final_val.get_type() == Variant::INT) p_final_val = p_final_val.operator real_t(); ERR_FAIL_COND_V(p_object == NULL, false); ERR_FAIL_COND_V(!ObjectDB::instance_validate(p_object), false); ERR_FAIL_COND_V(p_initial_val.get_type() != p_final_val.get_type(), false); ERR_FAIL_COND_V(p_times_in_sec <= 0, false); ERR_FAIL_COND_V(p_trans_type < 0 || p_trans_type >= TRANS_COUNT, false); ERR_FAIL_COND_V(p_ease_type < 0 || p_ease_type >= EASE_COUNT, false); ERR_FAIL_COND_V(p_delay < 0, false); bool prop_valid = false; p_object->get(p_property,&prop_valid); ERR_FAIL_COND_V(!prop_valid, false); InterpolateData data; data.active = true; data.type = INTER_PROPERTY; data.finish = false; data.elapsed = 0; data.id = p_object->get_instance_ID(); data.key = p_property; data.initial_val = p_initial_val; data.final_val = p_final_val; data.times_in_sec = p_times_in_sec; data.trans_type = p_trans_type; data.ease_type = p_ease_type; data.delay = p_delay; if(!_calc_delta_val(data.initial_val, data.final_val, data.delta_val)) return false; interpolates.push_back(data); return true; }
real_t acos(real_t const & x) { #if USE_GMPLIB return real_t(acos(x._data.get_d())); #else return real_t(acos(x._data)); #endif }
template<> real_t random<real_t>() { #if USE_GMPLIB return real_t(random_source.get_f(option_precision)); #else return real_t(rand()) / real_t(RAND_MAX); #endif }
real_t tan(real_t const & x) { #if USE_GMPLIB return real_t(tan(x._data.get_d())); #else return real_t(tan(x._data)); #endif }
bool Tween::interpolate_method(Object *p_object , String p_method , Variant p_initial_val , Variant p_final_val , real_t p_times_in_sec , TransitionType p_trans_type , EaseType p_ease_type , real_t p_delay ) { if(pending_update != 0) { _add_pending_command("interpolate_method" , p_object , p_method , p_initial_val , p_final_val , p_times_in_sec , p_trans_type , p_ease_type , p_delay ); return true; } // convert INT to REAL is better for interpolaters if(p_initial_val.get_type() == Variant::INT) p_initial_val = p_initial_val.operator real_t(); if(p_final_val.get_type() == Variant::INT) p_final_val = p_final_val.operator real_t(); ERR_FAIL_COND_V(p_object == NULL, false); ERR_FAIL_COND_V(p_initial_val.get_type() != p_final_val.get_type(), false); ERR_FAIL_COND_V(p_times_in_sec <= 0, false); ERR_FAIL_COND_V(p_trans_type < 0 || p_trans_type >= TRANS_COUNT, false); ERR_FAIL_COND_V(p_ease_type < 0 || p_ease_type >= EASE_COUNT, false); ERR_FAIL_COND_V(p_delay < 0, false); ERR_EXPLAIN("Object has no method named: %s" + p_method); ERR_FAIL_COND_V(!p_object->has_method(p_method), false); InterpolateData data; data.active = true; data.type = INTER_METHOD; data.finish = false; data.elapsed = 0; data.id = p_object->get_instance_ID(); data.key = p_method; data.initial_val = p_initial_val; data.final_val = p_final_val; data.times_in_sec = p_times_in_sec; data.trans_type = p_trans_type; data.ease_type = p_ease_type; data.delay = p_delay; if(!_calc_delta_val(data.initial_val, data.final_val, data.delta_val)) return false; interpolates.push_back(data); return true; }
void ViewFogGL3::changeStepEvent(const int newStep, const int oldStep) { real_t r = real_t(newStep)/real_t(oldStep); __start *= r; __end *= r; __fogw->EndEdit->setText(QString::number(__end)); __fogw->StartEdit->setText(QString::number(__start)); __density /= r; }
void OpenglApplication::render() { // adjust camera aspect int width, height; this->get_dimension( &width, &height ); camera_control.camera.aspect = real_t( width ) / real_t( height ); // render project.render( &camera_control.camera ); }
real_t abs(real_t const & x) { #if USE_GMPLIB return real_t(abs(x._data)); #else if (x._data < 0) return real_t(-x._data); return x; #endif }
void QuantisedFunction::computeCache(const Curve2DPtr& curve) { assert( __sampling > 2 ); __firstx = curve->getPointAt(curve->getFirstKnot()).x(); __lastx = curve->getPointAt(curve->getLastKnot()).x(); real_t extent = __lastx - __firstx; __values.resize(__sampling); for (uint_t i=0; i<__sampling; ++i) __values[i] = _computeValue(curve, extent * real_t(i)/real_t(__sampling-1) + __firstx); }
void VehicleBody::_update_suspension(PhysicsDirectBodyState *s) { real_t chassisMass = mass; for (int w_it=0; w_it<wheels.size(); w_it++) { VehicleWheel& wheel_info = *wheels[w_it]; if ( wheel_info.m_raycastInfo.m_isInContact ) { real_t force; // Spring { real_t susp_length = wheel_info.m_suspensionRestLength; real_t current_length = wheel_info.m_raycastInfo.m_suspensionLength; real_t length_diff = (susp_length - current_length); force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension; } // Damper { real_t projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; { real_t susp_damping; if ( projected_rel_vel < real_t(0.0) ) { susp_damping = wheel_info.m_wheelsDampingCompression; } else { susp_damping = wheel_info.m_wheelsDampingRelaxation; } force -= susp_damping * projected_rel_vel; } } // RESULT wheel_info.m_wheelsSuspensionForce = force * chassisMass; if (wheel_info.m_wheelsSuspensionForce < real_t(0.)) { wheel_info.m_wheelsSuspensionForce = real_t(0.); } } else { wheel_info.m_wheelsSuspensionForce = real_t(0.0); } } }
bool Tween::targeting_method(Object *p_object , String p_method , Object *p_initial , String p_initial_method , Variant p_final_val , real_t p_times_in_sec , TransitionType p_trans_type , EaseType p_ease_type , real_t p_delay ) { ERR_FAIL_COND_V(pending_update != 0, false); // convert INT to REAL is better for interpolaters if(p_final_val.get_type() == Variant::INT) p_final_val = p_final_val.operator real_t(); ERR_FAIL_COND_V(p_object == NULL, false); ERR_FAIL_COND_V(p_initial == NULL, false); ERR_FAIL_COND_V(p_times_in_sec <= 0, false); ERR_FAIL_COND_V(p_trans_type < 0 || p_trans_type >= TRANS_COUNT, false); ERR_FAIL_COND_V(p_ease_type < 0 || p_ease_type >= EASE_COUNT, false); ERR_FAIL_COND_V(p_delay < 0, false); ERR_FAIL_COND_V(!p_object->has_method(p_method), false); ERR_FAIL_COND_V(!p_initial->has_method(p_initial_method), false); Variant::CallError error; Variant initial_val = p_initial->call(p_initial_method, NULL, 0, error); ERR_FAIL_COND_V(error.error != Variant::CallError::CALL_OK, false); // convert INT to REAL is better for interpolaters if(initial_val.get_type() == Variant::INT) initial_val = initial_val.operator real_t(); ERR_FAIL_COND_V(initial_val.get_type() != p_final_val.get_type(), false); InterpolateData data; data.active = true; data.type = TARGETING_METHOD; data.finish = false; data.elapsed = 0; data.id = p_object->get_instance_ID(); data.key = p_method; data.target_id = p_initial->get_instance_ID(); data.target_key = p_initial_method; data.initial_val = initial_val; data.final_val = p_final_val; data.times_in_sec = p_times_in_sec; data.trans_type = p_trans_type; data.ease_type = p_ease_type; data.delay = p_delay; if(!_calc_delta_val(data.initial_val, data.final_val, data.delta_val)) return false; interpolates.push_back(data); return true; }
bool SliderJointSW::setup(float p_step) { //calculate transforms m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformB = B->get_transform() * m_frameInB; m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotBInW = m_calculatedTransformB.origin; m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X m_delta = m_realPivotBInW - m_realPivotAInW; m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; m_relPosA = m_projPivotInW - A->get_transform().origin; m_relPosB = m_realPivotBInW - B->get_transform().origin; Vector3 normalWorld; int i; //linear part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.basis.get_axis(i); memnew_placement(&m_jacLin[i], JacobianEntrySW( A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), m_relPosA, m_relPosB, normalWorld, A->get_inv_inertia(), A->get_inv_mass(), B->get_inv_inertia(), B->get_inv_mass() )); m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal(); m_depth[i] = m_delta.dot(normalWorld); } testLinLimits(); // angular part for(i = 0; i < 3; i++) { normalWorld = m_calculatedTransformA.basis.get_axis(i); memnew_placement(&m_jacAng[i], JacobianEntrySW( normalWorld, A->get_transform().basis.transposed(), B->get_transform().basis.transposed(), A->get_inv_inertia(), B->get_inv_inertia() )); } testAngLimits(); Vector3 axisA = m_calculatedTransformA.basis.get_axis(0); m_kAngle = real_t(1.0 )/ (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA)); // clear accumulator for motors m_accumulatedLinMotorImpulse = real_t(0.0); m_accumulatedAngMotorImpulse = real_t(0.0); return true; } // SliderJointSW::buildJacobianInt()
bool Generic6DOFJointSW::setup(float p_step) { // Clear accumulated impulses for the next simulation step m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.)); int i; for(i = 0; i < 3; i++) { m_angularLimits[i].m_accumulatedImpulse = real_t(0.); } //calculates transform calculateTransforms(); // const Vector3& pivotAInW = m_calculatedTransformA.origin; // const Vector3& pivotBInW = m_calculatedTransformB.origin; calcAnchorPos(); Vector3 pivotAInW = m_AnchorPos; Vector3 pivotBInW = m_AnchorPos; // not used here // Vector3 rel_pos1 = pivotAInW - A->get_transform().origin; // Vector3 rel_pos2 = pivotBInW - B->get_transform().origin; Vector3 normalWorld; //linear part for (i=0;i<3;i++) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { if (m_useLinearReferenceFrameA) normalWorld = m_calculatedTransformA.basis.get_axis(i); else normalWorld = m_calculatedTransformB.basis.get_axis(i); buildLinearJacobian( m_jacLinear[i],normalWorld , pivotAInW,pivotBInW); } } // angular part for (i=0;i<3;i++) { //calculates error angle if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i)) { normalWorld = this->getAxis(i); // Create angular atom buildAngularJacobian(m_jacAng[i],normalWorld); } } return true; }
void GlslApplication::render() { // adjust camera aspect int width, height; this->get_dimension( &width, &height ); glViewport( 0, 0, width, height ); camera_control.camera.aspect = real_t( width ) / real_t( height ); // render project.render(); }
// pressure as a function of "theta times dry air density" and water vapour mixing ratio phc_declare_funct_macro quantity<si::pressure, real_t> p( const quantity<multiply_typeof_helper<si::mass_density, si::temperature>::type, real_t> rhod_th, quantity<mixing_ratio, real_t> r // TODO: const ) { return p_1000<real_t>() * real_t(pow( (rhod_th * R_d<real_t>()) / p_1000<real_t>() * (real_t(1) + r / eps<real_t>()), 1 / (1 - R_over_c_p(r)) )); }
void Generic6DOFJointSW::solve(real_t timeStep) { m_timeStep = timeStep; //calculateTransforms(); int i; // linear Vector3 pointInA = m_calculatedTransformA.origin; Vector3 pointInB = m_calculatedTransformB.origin; real_t jacDiagABInv; Vector3 linear_axis; for (i=0;i<3;i++) { if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i)) { jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal(); if (m_useLinearReferenceFrameA) linear_axis = m_calculatedTransformA.basis.get_axis(i); else linear_axis = m_calculatedTransformB.basis.get_axis(i); m_linearLimits.solveLinearAxis( m_timeStep, jacDiagABInv, A,pointInA, B,pointInB, i,linear_axis, m_AnchorPos); } } // angular Vector3 angular_axis; real_t angularJacDiagABInv; for (i=0;i<3;i++) { if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques()) { // get axis angular_axis = getAxis(i); angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal(); m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B); } } }