SphereLDS::SphereLDS(double r, double m,
SP::SiconosVector qinit,
SP::SiconosVector vinit)
: LagrangianDS(qinit, vinit), radius(r), massValue(m)
{
normalize(q(), 3);
normalize(q(), 4);
normalize(q(), 5);
_ndof = 6;
assert(qinit->size() == _ndof);
assert(vinit->size() == _ndof);
_mass.reset(new SimpleMatrix(_ndof, _ndof));
_mass->zero();
I = massValue * radius * radius * 2. / 5.;
(*_mass)(0, 0) = (*_mass)(1, 1) = (*_mass)(2, 2) = massValue; ;
(*_mass)(3, 3) = (*_mass)(4, 4) = (*_mass)(5, 5) = I;
computeMass();
_jacobianFGyrq.reset(new SimpleMatrix(_ndof, _ndof));
_jacobianFGyrqDot.reset(new SimpleMatrix(_ndof, _ndof));
_fGyr.reset(new SiconosVector(_ndof));
_fGyr->zero();
computeFGyr();
}
void SphSystemData2::setTargetSpacing(double spacing) {
ParticleSystemData2::setRadius(spacing);
_targetSpacing = spacing;
_kernelRadius = _kernelRadiusOverTargetSpacing * _targetSpacing;
computeMass();
}
void btSphere::setRadius(btScalar radius)
{
if(shape != NULL)
{
delete shape;
shape = new btSphereShape(radius);
rigidBody->setCollisionShape(shape);
btScalar mass = computeMass();
btVector3 intertia;
this->shape->calculateLocalInertia(mass,intertia);
this->rigidBody->setMassProps(mass,intertia);
}
}
void btSphere::init(btScalar radius, btScalar density, const btTransform &transform)
{
this->density = density;
// this->size = size;
// this->initialPosition = position;
// this->initiaEulerlRotation = EulerRotation;
// shape
this->shape = new btSphereShape(radius);
// rotation with euler method :) (warning order Z-Y-X)
this->motionState = new btDefaultMotionState(transform);
// body
btScalar mass = computeMass();
btVector3 fallInertia(0,0,0);
this->shape->calculateLocalInertia(mass,fallInertia);
this->rigidBody = new btRigidBody(mass,this->motionState,this->shape,fallInertia);
origin = new RigidBodyOrigin(RigidBodyOrigin::BASIC_SHAPE,(QObject *)this);
rigidBody->setUserPointer(origin);
}
void SphSystemData2::setTargetDensity(double targetDensity) {
_targetDensity = targetDensity;
computeMass();
}
void SphSystemData2::setRelativeKernelRadius(double relativeRadius) {
_kernelRadiusOverTargetSpacing = relativeRadius;
_kernelRadius = _kernelRadiusOverTargetSpacing * _targetSpacing;
computeMass();
}
