C++ VRTransformPtr::getPhysics方法代码示例

void virtuose::attachTransform(VRTransformPtr trans)
{
    if(vc == 0) return;

    isAttached = true;
    attached = trans;
    VRPhysics* o = trans->getPhysics();
    btMatrix3x3 t = o->getInertiaTensor();
    float inertia[9] {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
    Matrix3ToArray(t,inertia);
    cout<<"\n "<<"\n "<<inertia[0] << "    " <<inertia[1] <<  "    " <<inertia[2] << "\n "<<inertia[3] <<  "    " <<inertia[4] <<  "    " <<inertia[5] << "\n "<<inertia[6] << "    " <<inertia[7] <<"    " << inertia[8]<<"\n ";
    CHECK(virtAttachVO(vc, o->getMass(), inertia));

}

// called from VRTransform::apply_constraints
void VRConstraint::apply(VRTransformPtr obj, VRObjectPtr parent, bool force) {
    if (!active || obj->getPhysics()->isPhysicalized()) return;
    auto now = VRGlobals::CURRENT_FRAME;
    if (apply_time_stamp == now && !force) return;
    apply_time_stamp = now;

    if (local) parent = obj->getParent(true);
    if (auto r = Referential.lock()) parent = r;

    Matrix4d J;
    if (parent) J = parent->getMatrixTo(obj);
    else J = obj->getWorldMatrix();
    J.mult(refMatrixB);
    J.multLeft(refMatrixAI);

    for (int i=0; i<3; i++) { // translation
        if (min[i] > max[i]) continue; // free
        if (min[i] > J[3][i]) J[3][i] = min[i]; // lower bound
        if (max[i] < J[3][i]) J[3][i] = max[i]; // upper bound
    }

    Vec3d angles = VRTransform::computeEulerAngles(J);

    auto sign = [](float a) {
        return a<0?-1:1;
    };

    // TODO: this is not correct, for example [180, 20, 180], corresponds to [0, 160, 0], and not [0, 20, 0] !!
    //  this tries to fix it somewhat, but its not clean!
    if ( abs(angles[0]) > Pi*0.5 && abs(angles[2]) > Pi*0.5) {
        angles[0] -= sign(angles[0])*Pi;
        angles[2] -= sign(angles[2])*Pi;
        angles[1] = Pi - angles[1];
    }

    Vec3d angleDiff;
    for (int i=3; i<6; i++) { // rotation
        if (min[i] > max[i]) continue; // free
        float a = angles[i-3];
        float d1 = min[i]-a; while(d1 > Pi) d1 -= 2*Pi; while(d1 < -Pi) d1 += 2*Pi;
        float d2 = max[i]-a; while(d2 > Pi) d2 -= 2*Pi; while(d2 < -Pi) d2 += 2*Pi;
        if (d1 > 0 && abs(d1) <= abs(d2)) angleDiff[i-3] = d1; // lower bound
        if (d2 < 0 && abs(d2) <= abs(d1)) angleDiff[i-3] = d2; // upper bound
    }
    VRTransform::applyEulerAngles(J, angles + angleDiff);

    J.multLeft(refMatrixA);
    J.mult(refMatrixBI);
    obj->setMatrixTo(J, parent);
}

void virtuose::attachTransform(VRTransformPtr trans) {
    //if(!connected()) return;
    isAttached = true;
    attached = trans;
    VRPhysics* o = trans->getPhysics();
    btMatrix3x3 t = o->getInertiaTensor();
    Vec9 inertia;
    Matrix3ToArray(t,inertia.data);
    print(inertia, 9);
    //cout<<"\n virtuose::attachTransform:\n " << inertia[0] << "    " <<inertia[1] <<  "    " <<inertia[2] << "\n "<<inertia[3] <<  "    " <<inertia[4] <<  "    " <<inertia[5] << "\n "<<inertia[6] << "    " <<inertia[7] <<"    " << inertia[8]<<"\n ";
    cout<<"\n virtuose::attachTransform:\n ";
    interface.setObject<Vec9>("inertia", inertia);
    interface.setObject<float>("mass", o->getMass());
    interface.setObject<bool>("doAttach", true);
    //CHECK(virtAttachVO(vc, o->getMass(), inertia));
}

void VRPhysicsManager::unphysicalize(VRTransformPtr obj) {
    if (!obj) return;
    btCollisionObject* bdy = obj->getPhysics()->getCollisionObject();
    if (!bdy) return;
    if (OSGobjs.count(bdy)) OSGobjs.erase(bdy);
}

void VRPhysicsManager::physicalize(VRTransformPtr obj) {
    if (!obj) return;
    btCollisionObject* bdy = obj->getPhysics()->getCollisionObject();
    if (!bdy) return;
    OSGobjs[bdy] = obj;
}