コード例 #1
0
ファイル: PhysicsEngine.cpp プロジェクト: PtrMan/SpaceSimCore
void PhysicsEngine::calcForcesAndReactionsForRigidBodies(float timeDelta) {
    for( int index = 0; index < physicsBodies.getCount(); index++ ) {
        SharedPointer<PhysicsBody> currentBody = physicsBodies[index];

        currentBody->angularVelocityDelta = Eigen::Matrix<double, 3, 1>(0.0, 0.0, 0.0);

        // TODO< put into method >

        for( int attachedForceIterator = 0; attachedForceIterator < currentBody->attachedForces.getCount(); attachedForceIterator++ ) {
            SharedPointer<AttachedForce> currentAttachedForce = currentBody->attachedForces[attachedForceIterator];

            // TODO< split each force into linear and angular components >

            Eigen::Matrix<double, 3, 1> angularComponentForce = currentAttachedForce->forceInNewton;

            Eigen::Matrix<double, 3, 1> appliedTorque = calculateTorque(currentAttachedForce->objectLocalPosition, angularComponentForce);
            Eigen::Matrix<double, 3, 1> rotationalAcceleration = calculateRotationalAcceleration(currentBody->inertiaTensor, appliedTorque);

            currentBody->angularVelocityDelta += (rotationalAcceleration * timeDelta);
        }


        static_cast<AccelerationImplementation*>(rungeKutta4.acceleration)->setCurrentBodyMass(currentBody->mass);
        rungeKutta4.integrate(currentBody->rungeKuttaState, 0.0, static_cast<double>(timeDelta));


    }

    // TODO< put into own method >
    for( int index = 0; index < fastParticles.getCount(); index++ ) {
        SharedPointer<FastParticle> currentParticle = fastParticles[index];

        currentParticle->nextCurrentPosition = currentParticle->currentPosition + (currentParticle->velocity * timeDelta);
    }
}
コード例 #2
0
void ODE_LinearSpringModel::updateInputValues() {

	if(mOwnerSpringAdapter == 0 || mOwnerSpringAdapter->getCurrentTargetJoint() == 0) {
		return;
	}

	//The first time an update is run the mJoint is located. 
	//This has to be done here instead of setup() to ensure that the joint is
 	//really available, which can not be guaranteed in setup().
	if(mJoint == 0) {
		SimJoint *simJoint = mOwnerSpringAdapter->getCurrentTargetJoint();
		
		ODE_Joint *odeJoint = dynamic_cast<ODE_Joint*>(simJoint);

		if(odeJoint == 0) {
			MotorAdapter *adapter = dynamic_cast<MotorAdapter*>(simJoint);
			if(adapter != 0) {
				odeJoint = dynamic_cast<ODE_Joint*>(adapter->getActiveMotorModel());
			}
		}

		if(simJoint != 0 && odeJoint != 0) {
			if(getType() == MotorModel::HINGE_JOINT) {
				if(dynamic_cast<HingeJoint*>(simJoint) != 0) {
					mJoint = odeJoint->getJoint();
				}
			}
			else if(getType() == MotorModel::SLIDER_JOINT) {
				if(dynamic_cast<SliderJoint*>(simJoint) != 0) {
					mJoint = odeJoint->getJoint();
				}
			}
		}
	}

	if(mJoint != 0) {

		if(getType() == MotorModel::HINGE_JOINT) {
			double torque = calculateTorque(dJointGetHingeAngle(mJoint));
			dJointAddHingeTorque(mJoint, torque);
			mOwnerSpringAdapter->getCurrentTorqueValue()->set(torque);
		}
		else if(getType() == MotorModel::SLIDER_JOINT) {
			double force = calculateForce(dJointGetSliderPosition(mJoint));
			dJointAddSliderForce(mJoint, force);
			mOwnerSpringAdapter->getCurrentTorqueValue()->set(force);
		}
	}
}