VALUE MSNewton::Slider::set_controller(VALUE self, VALUE v_joint, VALUE v_controller) {
JointData* joint_data = Util::value_to_joint2(v_joint, JT_SLIDER);
SliderData* cj_data = (SliderData*)joint_data->cj_data;
dFloat controller = Util::value_to_dFloat(v_controller);
if (controller != cj_data->controller) {
cj_data->controller = controller;
if (joint_data->connected) NewtonBodySetSleepState(joint_data->child, 0);
}
return Util::to_value(cj_data->controller);
}
VALUE MSP::PointToPoint::rbf_set_controller(VALUE self, VALUE v_joint, VALUE v_controller) {
MSP::Joint::JointData* joint_data = MSP::Joint::c_value_to_joint2(v_joint, MSP::Joint::POINT_TO_POINT);
PointToPointData* cj_data = reinterpret_cast<PointToPointData*>(joint_data->m_cj_data);
dFloat desired_controller = Util::max_float(Util::value_to_dFloat(v_controller), 0.0f);
if (desired_controller != cj_data->m_controller) {
cj_data->m_controller = desired_controller;
if (joint_data->m_connected)
NewtonBodySetSleepState(joint_data->m_child, 0);
}
return Qnil;
}
VALUE MSP::BallAndSocket::rbf_set_controller(VALUE self, VALUE v_joint, VALUE v_controller) {
MSP::Joint::JointData* joint_data = MSP::Joint::c_value_to_joint2(v_joint, MSP::Joint::BALL_AND_SOCKET);
BallAndSocketData* cj_data = reinterpret_cast<BallAndSocketData*>(joint_data->m_cj_data);
dFloat desired_controller = Util::value_to_dFloat(v_controller);
if (cj_data->m_controller != desired_controller) {
cj_data->m_controller = desired_controller;
if (joint_data->m_connected)
NewtonBodySetSleepState(joint_data->m_child, 0);
}
return Qnil;
}
VALUE MSP::PointToPoint::rbf_set_start_distance(VALUE self, VALUE v_joint, VALUE v_distance) {
MSP::Joint::JointData* joint_data = MSP::Joint::c_value_to_joint2(v_joint, MSP::Joint::POINT_TO_POINT);
PointToPointData* cj_data = reinterpret_cast<PointToPointData*>(joint_data->m_cj_data);
dFloat des_start_distance = Util::max_float(Util::value_to_dFloat(v_distance) * M_METER_TO_INCH, 0.0f);
if (des_start_distance != cj_data->m_start_distance) {
cj_data->m_start_distance = des_start_distance;
if (joint_data->m_connected)
NewtonBodySetSleepState(joint_data->m_child, 0);
}
return Qnil;
}
VALUE MSNewton::Servo::set_controller(VALUE self, VALUE v_joint, VALUE v_controller) {
JointData* joint_data = Util::value_to_joint2(v_joint, JT_SERVO);
ServoData* cj_data = (ServoData*)joint_data->cj_data;
if (v_controller == Qnil) {
if (cj_data->controller_enabled == true) {
cj_data->controller_enabled = false;
if (joint_data->connected)
NewtonBodySetSleepState(joint_data->child, 0);
}
return Qnil;
}
else {
dFloat controller = Util::value_to_dFloat(v_controller);
if (cj_data->controller_enabled == false || controller != cj_data->controller) {
cj_data->controller_enabled = true;
cj_data->controller = controller;
if (joint_data->connected)
NewtonBodySetSleepState(joint_data->child, 0);
}
return Util::to_value(cj_data->controller);
}
}
void dCustomKinematicController::Init (NewtonBody* const body, const dMatrix& matrix)
{
CalculateLocalMatrix(matrix, m_localMatrix0, m_localMatrix1);
m_autoSleepState = NewtonBodyGetAutoSleep(body) ? true : false;
NewtonBodySetSleepState(body, 0);
SetPickMode(1);
SetLimitRotationVelocity(10.0f);
SetTargetMatrix(matrix);
SetMaxLinearFriction(1.0f);
SetMaxAngularFriction(1.0f);
// set as soft joint
SetSolverModel(2);
}
void NzPhysObject::AddTorque(const NzVector3f& torque, nzCoordSys coordSys)
{
switch (coordSys)
{
case nzCoordSys_Global:
m_torqueAccumulator += torque;
break;
case nzCoordSys_Local:
m_torqueAccumulator += m_matrix.Transform(torque, 0.f);
break;
}
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
NewtonBodySetSleepState(m_body, 0);
}
void NzPhysObject::AddForce(const NzVector3f& force, nzCoordSys coordSys)
{
switch (coordSys)
{
case nzCoordSys_Global:
m_forceAccumulator += force;
break;
case nzCoordSys_Local:
m_forceAccumulator += GetRotation()*force;
break;
}
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
NewtonBodySetSleepState(m_body, 0);
}
void MoveToTarget (NewtonBody* const targetBody)
{
dAssert ((targetBody == m_triggerPort0) || (targetBody == m_triggerPort1));
if (targetBody != m_currentPort) {
m_currentPort = targetBody;
// get the location of next target
dMatrix targetMatrix;
NewtonBodyGetMatrix(m_currentPort, &targetMatrix[0][0]);
m_target = targetMatrix.m_posit;
// the body might be sleep we need to activate the body by sett the sleep stet off
NewtonBodySetSleepState(GetBody0(), 0);
m_state = m_driving;
}
}
void NzPhysObject::AddForce(const NzVector3f& force, const NzVector3f& point, nzCoordSys coordSys)
{
switch (coordSys)
{
case nzCoordSys_Global:
m_forceAccumulator += force;
m_torqueAccumulator += NzVector3f::CrossProduct(point - GetMassCenter(nzCoordSys_Global), force);
break;
case nzCoordSys_Local:
AddForce(m_matrix.Transform(force, 0.f), m_matrix.Transform(point));
return;
}
// On réveille le corps pour que le callback soit appelé et que les forces soient appliquées
NewtonBodySetSleepState(m_body, 0);
}
void DemoCameraListener::UpdatePickBody(DemoEntityManager* const scene, dFloat timestep)
{
NewtonDemos* const mainWin = scene->GetRootWindow();
// handle pick body from the screen
bool mousePickState = mainWin->GetMouseKeyState(0);
if (!m_targetPicked) {
if (!m_prevMouseState && mousePickState) {
dFloat param;
dVector posit;
dVector normal;
dFloat x = dFloat (m_mousePosX);
dFloat y = dFloat (m_mousePosY);
dVector p0 (m_camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
dVector p1 (m_camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
NewtonBody* const body = MousePickByForce (scene->GetNewton(), p0, p1, param, posit, normal);
if (body) {
m_targetPicked = body;
dMatrix matrix;
NewtonBodyGetMatrix(m_targetPicked, &matrix[0][0]);
// save point local to the body matrix
m_pickedBodyParam = param;
m_pickedBodyLocalAtachmentPoint = matrix.UntransformVector (posit);
// convert normal to local space
m_pickedBodyLocalAtachmentNormal = matrix.UnrotateVector(normal);
// link the a destructor callback
//m_bodyDestructor = NewtonBodyGetDestructorCallback(m_targetPicked);
//NewtonBodySetDestructorCallback(m_targetPicked, OnPickedBodyDestroyedNotify);
}
}
} else {
if (mainWin->GetMouseKeyState(0)) {
dFloat x = dFloat (m_mousePosX);
dFloat y = dFloat (m_mousePosY);
dVector p0 (m_camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
dVector p1 (m_camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
m_pickedBodyTargetPosition = p0 + (p1 - p0).Scale (m_pickedBodyParam);
dMatrix matrix;
NewtonBodyGetMatrix (m_targetPicked, &matrix[0][0]);
dVector point (matrix.TransformVector(m_pickedBodyLocalAtachmentPoint));
CalculatePickForceAndTorque (m_targetPicked, point, m_pickedBodyTargetPosition, timestep);
} else {
if (m_targetPicked) {
NewtonBodySetSleepState (m_targetPicked, 0);
}
// unchain the callbacks
//NewtonBodySetDestructorCallback(m_targetPicked, m_bodyDestructor);
m_targetPicked = NULL;
m_bodyDestructor = NULL;
}
}
m_prevMouseState = mousePickState;
}
void dCustomKinematicController::SetTargetRotation(const dQuaternion& rotation)
{
NewtonBodySetSleepState(m_body0, 0);
m_targetMatrix = dMatrix(rotation, m_targetMatrix.m_posit);
}
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
dVector com;
dMatrix matrix;
dVector omega0;
dVector veloc0;
dVector omega1;
dVector veloc1;
dVector pointVeloc;
const dFloat stiffness = 0.3f;
const dFloat angularDamp = 0.95f;
dFloat invTimeStep = 1.0f / timestep;
NewtonWorld* const world = NewtonBodyGetWorld (body);
NewtonWorldCriticalSectionLock (world, 0);
// calculate the desired impulse
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetOmega (body, &omega0[0]);
NewtonBodyGetVelocity (body, &veloc0[0]);
NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);
dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
for (int i = 0; i < 3; i ++) {
dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
veloc[i] = deltaVeloc[i];
NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
}
// damp angular velocity
NewtonBodyGetOmega (body, &omega1[0]);
NewtonBodyGetVelocity (body, &veloc1[0]);
omega1 = omega1.Scale (angularDamp);
// restore body velocity and angular velocity
NewtonBodySetOmega (body, &omega0[0]);
NewtonBodySetVelocity(body, &veloc0[0]);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector angularMomentum (Ixx, Iyy, Izz);
angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));
dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
dVector torque (angularMomentum.Scale(invTimeStep));
NewtonBodyAddForce(body, &force[0]);
NewtonBodyAddTorque(body, &torque[0]);
// make sure the body is unfrozen, if it is picked
NewtonBodySetSleepState (body, 0);
NewtonWorldCriticalSectionUnlock (world);
}
void dCustomKinematicController::SetTargetPosit(const dVector& posit)
{
NewtonBodySetSleepState(m_body0, 0);
m_targetMatrix.m_posit = posit;
m_targetMatrix.m_posit.m_w = 1.0f;
}
void dNewtonBody::SetSleepState(bool state) const
{
NewtonBodySetSleepState(m_body, state ? 1 : 0);
}
void dCustomKinematicController::SetTargetMatrix(const dMatrix& matrix)
{
NewtonBodySetSleepState(m_body0, 0);
m_targetMatrix = matrix;
m_targetMatrix.m_posit.m_w = 1.0f;
}
void DemoCameraListener::UpdatePickBody(DemoEntityManager* const scene, dFloat timestep)
{
// handle pick body from the screen
bool mousePickState = scene->GetMouseKeyState(0);
if (!m_targetPicked) {
if (!m_prevMouseState && mousePickState) {
dFloat param;
dVector posit;
dVector normal;
dFloat x = dFloat (m_mousePosX);
dFloat y = dFloat (m_mousePosY);
dVector p0 (m_camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
dVector p1 (m_camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
NewtonBody* const body = MousePickBody (scene->GetNewton(), p0, p1, param, posit, normal);
if (body) {
dMatrix matrix;
m_targetPicked = body;
NewtonBodyGetMatrix(m_targetPicked, &matrix[0][0]);
m_pickedBodyParam = param;
#ifdef USE_PICK_BODY_BY_FORCE
// save point local to the body matrix
m_pickedBodyLocalAtachmentPoint = matrix.UntransformVector (posit);
// convert normal to local space
m_pickedBodyLocalAtachmentNormal = matrix.UnrotateVector(normal);
#else
if(m_pickJoint) {
delete m_pickJoint;
m_pickJoint = NULL;
}
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
const dFloat angularFritionAccel = 100.0f;
const dFloat linearFrictionAccel = 100.0f * dAbs (dMax (DEMO_GRAVITY, 10.0f));
const dFloat inertia = dMax (Izz, dMax (Ixx, Iyy));
m_pickJoint = new dCustomKinematicController (body, posit);
m_pickJoint->SetPickMode (0);
m_pickJoint->SetMaxLinearFriction(mass * linearFrictionAccel);
m_pickJoint->SetMaxAngularFriction(inertia * angularFritionAccel);
#endif
}
}
} else {
if (scene->GetMouseKeyState(0)) {
dFloat x = dFloat (m_mousePosX);
dFloat y = dFloat (m_mousePosY);
dVector p0 (m_camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
dVector p1 (m_camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
m_pickedBodyTargetPosition = p0 + (p1 - p0).Scale (m_pickedBodyParam);
#ifdef USE_PICK_BODY_BY_FORCE
dMatrix matrix;
NewtonBodyGetMatrix (m_targetPicked, &matrix[0][0]);
dVector point (matrix.TransformVector(m_pickedBodyLocalAtachmentPoint));
CalculatePickForceAndTorque (m_targetPicked, point, m_pickedBodyTargetPosition, timestep);
#else
if (m_pickJoint) {
m_pickJoint->SetTargetPosit (m_pickedBodyTargetPosition);
}
#endif
} else {
if (m_targetPicked) {
NewtonBodySetSleepState (m_targetPicked, 0);
}
if (m_pickJoint) {
delete m_pickJoint;
}
m_pickJoint = NULL;
m_targetPicked = NULL;
m_bodyDestructor = NULL;
}
}
m_prevMouseState = mousePickState;
}
