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