void VRPhysics::updateConstraint(VRPhysics* p) {
if (body == 0) return;
if (p->body == 0) return;
if (joints.count(p) == 0) return;
VRPhysicsJoint* joint = joints[p];
OSG::VRConstraint* c = joint->constraint;
if (c == 0) return;
Lock lock(mtx());
if (joint->btJoint != 0) {
world->removeConstraint(joint->btJoint);
delete joint->btJoint;
joint->btJoint = 0;
}
//the two world transforms
btTransform localA = btTransform();
localA.setIdentity();
btTransform localB = btTransform();
localB.setIdentity();
//Constraint.getReferenceFrameInB
localA = fromMatrix( c->getReferenceA() );
localB = fromMatrix( c->getReferenceB() );
// TODO: possible bug - p is not valid, may have been deleted!
//cout << "\nCreate Joint " << fromTransform(body->getWorldTransform())[3] << " " << fromTransform(p->body->getWorldTransform())[3] << endl;
//btTransform t = p->body->getWorldTransform().inverse();
//t.mult(t, body->getWorldTransform()); // the position of the first object in the local coords of the second
joint->btJoint = new btGeneric6DofSpringConstraint(*body, *p->body, localA, localB, true);
world->addConstraint(joint->btJoint, true);
for (int i=0; i<6; i++) {
joint->btJoint->setParam(BT_CONSTRAINT_STOP_CFM, 0, i);
joint->btJoint->setParam(BT_CONSTRAINT_STOP_ERP, 0.6, i);
}
joint->btJoint->setLinearLowerLimit(btVector3(c->getMin(0), c->getMin(1), c->getMin(2)));
joint->btJoint->setLinearUpperLimit(btVector3(c->getMax(0), c->getMax(1), c->getMax(2)));
joint->btJoint->setAngularLowerLimit(btVector3(c->getMin(3), c->getMin(4), c->getMin(5)));
joint->btJoint->setAngularUpperLimit(btVector3(c->getMax(3), c->getMax(4), c->getMax(5)));
// SPRING PARAMETERS
OSG::VRConstraint* cs = joint->spring;
if (cs == 0) return;
for (int i=0; i<6; i++) {
bool b = (cs->getMin(i) > 0);
float stiffness = cs->getMin(i);
float damping = cs->getMax(i);
joint->btJoint->enableSpring(i, b);
joint->btJoint->setStiffness(i, stiffness);
joint->btJoint->setDamping(i, damping);
joint->btJoint->setEquilibriumPoint(i);
}
}
Quat_<Real>& Quat_<Real>::fromAxes(const Vec3& axisX, const Vec3& axisY, const Vec3& axisZ)
{
Matrix4 rot;
rot.col(0) = Vec4(axisX);
rot.col(1) = Vec4(axisY);
rot.col(2) = Vec4(axisZ);
return fromMatrix(rot);
}
RoopList ResizeImage::execute(RoopMachine &machine, RoopList arguments) {
const size_t argsize = arguments.size();
cv::Mat newImage;
cv::Mat original = arguments[0].resultMat;
int newSizeX;
int newSizeY;
if (argsize < 2) {
machine.exceptionBitSet = true;
return RoopList();
}
else if (argsize == 2) {
newSizeX = argAsInt(arguments[1]);
newSizeY = argAsInt(arguments[1]);
} else {
newSizeX = argAsInt(arguments[1]);
newSizeY = argAsInt(arguments[2]);
}
cv::resize(original, newImage, cv::Size(newSizeX, newSizeY));
return fromMatrix(newImage);
}
Quaternion::Quaternion(const Matrix4 &m) {
fromMatrix(m.getRotation());
}
Quaternion::Quaternion(const Matrix3 &m) {
fromMatrix(m);
normalize();
}
RoopList LoadImage::execute(RoopMachine &machine, RoopList arguments) {
cv::Mat image;
std::cout << "Loading image " << arguments[0].resultString;
image = cv::imread(arguments[0].resultString, 1);
return fromMatrix(image);
}
RoopList SaveImage::execute(RoopMachine &machine, RoopList arguments) {
cv::Mat image = arguments[0].resultMat;
std::string filename = arguments[1].resultString;
cv::imwrite(filename, image);
return fromMatrix(image);
}
/*****************
*Axes Conversion*
*****************/
void Quaternion::fromAxes(const Vector3 &xAxis, const Vector3 &yAxis, const Vector3 &zAxis) {
fromMatrix(Matrix::FromAxes(xAxis, yAxis, zAxis));
}
Quaternion::Quaternion(const Matrix &m) { fromMatrix(m); }
