diff --git a/newton-4.00/applications/media/robot.fbx b/newton-4.00/applications/media/robot.fbx index 90d65cd43..d9e113baf 100644 Binary files a/newton-4.00/applications/media/robot.fbx and b/newton-4.00/applications/media/robot.fbx differ diff --git a/newton-4.00/applications/media/robot_old.fbx b/newton-4.00/applications/media/robot_old.fbx new file mode 100644 index 000000000..90d65cd43 Binary files /dev/null and b/newton-4.00/applications/media/robot_old.fbx differ diff --git a/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp b/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp index 999a8cdcb..94892ceb0 100644 --- a/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp +++ b/newton-4.00/applications/ndSandbox/demos/ndAdvancedIndustrialRobot.cpp @@ -1277,7 +1277,7 @@ void ndAdvancedIndustrialRobot(ndDemoEntityManager* const scene) ndVector origin1(0.0f, 0.0f, 0.0f, 1.0f); ndMeshLoader loader; - ndSharedPtr modelMesh(loader.LoadEntity("robot.fbx", scene)); + ndSharedPtr modelMesh(loader.LoadEntity("robot_old.fbx", scene)); ndMatrix matrix(ndYawMatrix(-ndPi * 0.5f)); #ifdef ND_TRAIN_MODEL diff --git a/newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_2.cpp b/newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_2.cpp index adeb9ff58..f3e0511a7 100644 --- a/newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_2.cpp +++ b/newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_2.cpp @@ -863,31 +863,33 @@ namespace ndQuadruped_2 //#pragma optimize( "", off ) bool CalculateHolonomicReward() const { - bool isHolonomic = true; - const ndModelArticulation* const model = GetModel()->GetAsModelArticulation(); - for (ndModelArticulation::ndNode* node = model->GetRoot()->GetFirstIterator(); isHolonomic && node; node = node->GetNextIterator()) - { - const ndVector veloc(node->m_body->GetVelocity()); - const ndVector omega(node->m_body->GetOmega()); - ndFloat32 vMag2 = veloc.DotProduct(veloc).GetScalar(); - ndFloat32 wMag2 = omega.DotProduct(omega).GetScalar(); - isHolonomic = isHolonomic && (vMag2 < 100.0f); - isHolonomic = isHolonomic && (wMag2 < 400.0f); - } - if (!isHolonomic) - { - // catastrophic penalty, results in a immediate kill. - ndMemSet(m_controllerTrainer->m_rewardsMemories, 0.0f, sizeof(m_controllerTrainer->m_rewardsMemories) / sizeof(m_controllerTrainer->m_rewardsMemories[0])); - } - - for (ndInt32 i = 0; isHolonomic && (i < m_animPose.GetCount()); ++i) - { - const ndEffectorInfo* const info = &m_effectorsInfo[i]; - const ndIkSwivelPositionEffector* const effector = (ndIkSwivelPositionEffector*)*info->m_effector; - bool holonomic = effector->IsHolonomic(m_timestep); - isHolonomic = isHolonomic && holonomic; - } - return isHolonomic; + ndAssert(0); + return false; + //bool isHolonomic = true; + //const ndModelArticulation* const model = GetModel()->GetAsModelArticulation(); + //for (ndModelArticulation::ndNode* node = model->GetRoot()->GetFirstIterator(); isHolonomic && node; node = node->GetNextIterator()) + //{ + // const ndVector veloc(node->m_body->GetVelocity()); + // const ndVector omega(node->m_body->GetOmega()); + // ndFloat32 vMag2 = veloc.DotProduct(veloc).GetScalar(); + // ndFloat32 wMag2 = omega.DotProduct(omega).GetScalar(); + // isHolonomic = isHolonomic && (vMag2 < 100.0f); + // isHolonomic = isHolonomic && (wMag2 < 400.0f); + //} + //if (!isHolonomic) + //{ + // // catastrophic penalty, results in a immediate kill. + // ndMemSet(m_controllerTrainer->m_rewardsMemories, 0.0f, sizeof(m_controllerTrainer->m_rewardsMemories) / sizeof(m_controllerTrainer->m_rewardsMemories[0])); + //} + // + //for (ndInt32 i = 0; isHolonomic && (i < m_animPose.GetCount()); ++i) + //{ + // const ndEffectorInfo* const info = &m_effectorsInfo[i]; + // const ndIkSwivelPositionEffector* const effector = (ndIkSwivelPositionEffector*)*info->m_effector; + // bool holonomic = effector->IsHolonomic(m_timestep); + // isHolonomic = isHolonomic && holonomic; + //} + //return isHolonomic; } ndReal GetReward() const @@ -951,45 +953,46 @@ namespace ndQuadruped_2 void GetObservation(ndBrainFloat* const inputObservations) { + ndAssert(0); ndMemSet(inputObservations, 0.0f, ND_AGENT_INPUT_SIZE); - ndObservationVector& observation = *((ndObservationVector*)inputObservations); - for (ndInt32 i = 0; i < m_animPose.GetCount(); ++i) - { - const ndAnimKeyframe& keyFrame = m_animPose[i]; - const ndEffectorInfo* const info = (ndEffectorInfo*)keyFrame.m_userData; - - ndInt32 paramCount = 0; - ndJointBilateralConstraint::ndKinematicState kinematicState[16]; - paramCount += info->m_thigh->GetKinematicState(&kinematicState[paramCount]); - paramCount += info->m_calf->GetKinematicState(&kinematicState[paramCount]); - paramCount += info->m_foot->GetKinematicState(&kinematicState[paramCount]); - for (ndInt32 j = 0; j < paramCount; ++j) - { - observation.m_legs[i].m_state[j * 2 + 0] = ndBrainFloat(kinematicState[j].m_posit); - observation.m_legs[i].m_state[j * 2 + 1] = ndBrainFloat(kinematicState[j].m_velocity); - } - - const ndVector& posit = keyFrame.m_posit; - observation.m_legs[i].m_posit.m_x = ndReal (posit.m_x); - observation.m_legs[i].m_posit.m_y = ndReal (posit.m_y); - observation.m_legs[i].m_posit.m_z = ndReal (posit.m_z); - - observation.m_legs[i].m_prevPosit.m_x = ndReal (m_animPrevPose[i].m_x); - observation.m_legs[i].m_prevPosit.m_y = ndReal (m_animPrevPose[i].m_y); - observation.m_legs[i].m_prevPosit.m_z = ndReal (m_animPrevPose[i].m_z); - - m_animPrevPose[i] = posit; - - ndBrainFloat contactDist = FindContactDist(i); - bool isHolonomic = info->m_effector->IsHolonomic(m_timestep); - observation.m_legs[i].m_hasContact = contactDist; - observation.m_legs[i].m_isHolonomic = ndBrainFloat(isHolonomic ? 0.0f : 1.0f); - observation.m_legs[i].m_animSequence = ndBrainFloat(keyFrame.m_userParamFloat); - } - - observation.m_torso_x = m_control->m_x; - observation.m_torso_z = m_control->m_z; - observation.m_animSpeed = m_control->m_animSpeed; + //ndObservationVector& observation = *((ndObservationVector*)inputObservations); + //for (ndInt32 i = 0; i < m_animPose.GetCount(); ++i) + //{ + // const ndAnimKeyframe& keyFrame = m_animPose[i]; + // const ndEffectorInfo* const info = (ndEffectorInfo*)keyFrame.m_userData; + // + // ndInt32 paramCount = 0; + // ndJointBilateralConstraint::ndKinematicState kinematicState[16]; + // paramCount += info->m_thigh->GetKinematicState(&kinematicState[paramCount]); + // paramCount += info->m_calf->GetKinematicState(&kinematicState[paramCount]); + // paramCount += info->m_foot->GetKinematicState(&kinematicState[paramCount]); + // for (ndInt32 j = 0; j < paramCount; ++j) + // { + // observation.m_legs[i].m_state[j * 2 + 0] = ndBrainFloat(kinematicState[j].m_posit); + // observation.m_legs[i].m_state[j * 2 + 1] = ndBrainFloat(kinematicState[j].m_velocity); + // } + // + // const ndVector& posit = keyFrame.m_posit; + // observation.m_legs[i].m_posit.m_x = ndReal (posit.m_x); + // observation.m_legs[i].m_posit.m_y = ndReal (posit.m_y); + // observation.m_legs[i].m_posit.m_z = ndReal (posit.m_z); + // + // observation.m_legs[i].m_prevPosit.m_x = ndReal (m_animPrevPose[i].m_x); + // observation.m_legs[i].m_prevPosit.m_y = ndReal (m_animPrevPose[i].m_y); + // observation.m_legs[i].m_prevPosit.m_z = ndReal (m_animPrevPose[i].m_z); + // + // m_animPrevPose[i] = posit; + // + // ndBrainFloat contactDist = FindContactDist(i); + // bool isHolonomic = info->m_effector->IsHolonomic(m_timestep); + // observation.m_legs[i].m_hasContact = contactDist; + // observation.m_legs[i].m_isHolonomic = ndBrainFloat(isHolonomic ? 0.0f : 1.0f); + // observation.m_legs[i].m_animSequence = ndBrainFloat(keyFrame.m_userParamFloat); + //} + // + //observation.m_torso_x = m_control->m_x; + //observation.m_torso_z = m_control->m_z; + //observation.m_animSpeed = m_control->m_animSpeed; } void ApplyActions(const ndBrainFloat* const actions) diff --git a/newton-4.00/applications/ndSandbox/demos/ndSimpleIndustrialRobot.cpp b/newton-4.00/applications/ndSandbox/demos/ndSimpleIndustrialRobot.cpp index b85504f0c..ba97ba2d1 100644 --- a/newton-4.00/applications/ndSandbox/demos/ndSimpleIndustrialRobot.cpp +++ b/newton-4.00/applications/ndSandbox/demos/ndSimpleIndustrialRobot.cpp @@ -56,10 +56,10 @@ namespace ndSimpleRobot { "effector", ndDefinition::m_effector , 0.0f, 0.0f, 0.0f}, }; - #define ND_MIN_X_SPAND ndReal (-1.5f) - #define ND_MAX_X_SPAND ndReal ( 1.5f) - #define ND_MIN_Y_SPAND ndReal (-2.2f) - #define ND_MAX_Y_SPAND ndReal ( 1.5f) + #define ND_MIN_X_SPAND ndReal (-2.2f) + #define ND_MAX_X_SPAND ndReal ( 1.5f) + #define ND_MIN_Z_SPAND ndReal (-1.5f) + #define ND_MAX_Z_SPAND ndReal ( 1.5f) class ndRobotBodyNotify : public ndDemoEntityNotify { @@ -95,7 +95,7 @@ namespace ndSimpleRobot ,m_effectorReference(ndGetIdentityMatrix()) ,m_world(nullptr) ,m_x(0.0f) - ,m_y(0.0f) + ,m_z(0.0f) ,m_azimuth(0.0f) ,m_yaw(0.0f) ,m_pitch(0.0f) @@ -143,6 +143,9 @@ namespace ndSimpleRobot m_rightGripper = (ndJointSlider*)robot->FindByName("rightGripper")->m_joint->GetAsBilateral(); m_effector = (ndIk6DofEffector*)robot->FindLoopByName("effector")->m_joint->GetAsBilateral(); + m_baseLimitJoint = (ndJointHinge*)robot->FindByName("arm_0")->m_joint->GetAsBilateral(); + m_elbowLimitJoint = (ndJointHinge*)robot->FindByName("arm_1")->m_joint->GetAsBilateral(); + ndBodyDynamic* const rootBody = robot->GetRoot()->m_body->GetAsBodyDynamic(); ndDemoEntity* const rootEntity = (ndDemoEntity*)rootBody->GetNotifyCallback()->GetUserData(); ndDemoEntity* const effectorEntity = rootEntity->Find("effector"); @@ -163,13 +166,14 @@ namespace ndSimpleRobot ndMatrix CalculateTargetMatrix() const { ndFloat32 x = m_x + m_effectorReference.m_posit.m_x; - ndFloat32 y = m_y + m_effectorReference.m_posit.m_y; - ndFloat32 z = m_effectorReference.m_posit.m_z; + ndFloat32 y = m_effectorReference.m_posit.m_y; + ndFloat32 z = m_z + m_effectorReference.m_posit.m_z; - const ndMatrix aximuthMatrix(ndYawMatrix(m_azimuth)); + const ndMatrix aximuthMatrix(ndPitchMatrix(m_azimuth)); const ndVector localPosit(ndVector::m_wOne + aximuthMatrix.RotateVector(ndVector(x, y, z, ndFloat32(1.0f)))); ndMatrix targetMatrix(ndPitchMatrix(m_pitch) * ndYawMatrix(m_yaw) * ndRollMatrix(m_roll)); + targetMatrix.m_posit = localPosit; return targetMatrix; } @@ -194,59 +198,77 @@ namespace ndSimpleRobot const ndVector CalculateTargetPosit() const { const ndMatrix currentEffectorMatrix(m_effector->GetEffectorMatrix()); - + // intepolate in local space; - ndFloat32 azimuth = -ndAtan2(currentEffectorMatrix.m_posit.m_z, currentEffectorMatrix.m_posit.m_x); - - const ndVector localPosit(ndYawMatrix(-azimuth).RotateVector(currentEffectorMatrix.m_posit) - m_effectorReference.m_posit); - + ndFloat32 azimuth = -ndAtan2(currentEffectorMatrix.m_posit.m_y, currentEffectorMatrix.m_posit.m_z); + + const ndVector localPosit(ndPitchMatrix(-azimuth).RotateVector(currentEffectorMatrix.m_posit) - m_effectorReference.m_posit); + // move on the plane with constant speed ndFloat32 planeSpeed = 0.05f; ndFloat32 dx = m_x - localPosit.m_x; - ndFloat32 dy = m_y - localPosit.m_y; - ndFloat32 mag = ndSqrt(dx * dx + dy * dy); + ndFloat32 dz = m_z - localPosit.m_z; + ndFloat32 mag = ndSqrt(dx * dx + dz * dz); ndFloat32 invPositMag = 1.0f; if (mag > planeSpeed) { invPositMag = planeSpeed / mag; } ndFloat32 x = localPosit.m_x + dx * invPositMag; - ndFloat32 y = localPosit.m_y + dy * invPositMag; - - ndFloat32 azimuthSpeed = 3.0f * ndDegreeToRad; + ndFloat32 z = localPosit.m_z + dz * invPositMag; + + ndFloat32 azimuthSpeed = 5.0f * ndDegreeToRad; ndFloat32 deltaAzimuth = ndAnglesSub(m_azimuth, azimuth); if (ndAbs(deltaAzimuth) > azimuthSpeed) { deltaAzimuth = azimuthSpeed * ndSign(deltaAzimuth); } ndFloat32 angle = azimuth + deltaAzimuth; - + // now calculate the in between matrix; ndFloat32 x1 = x + m_effectorReference.m_posit.m_x; - ndFloat32 y1 = y + m_effectorReference.m_posit.m_y; - ndFloat32 z1 = m_effectorReference.m_posit.m_z; - - const ndMatrix azimuthMatrix1(ndYawMatrix(angle)); - return ndVector::m_wOne + azimuthMatrix1.RotateVector(ndVector(x1, y1, z1, ndFloat32(1.0f))); + ndFloat32 y1 = m_effectorReference.m_posit.m_y; + ndFloat32 z1 = z + m_effectorReference.m_posit.m_z; + + const ndMatrix azimuthMatrix1(ndPitchMatrix(angle)); + const ndVector posit(ndVector::m_wOne + azimuthMatrix1.RotateVector(ndVector(x1, y1, z1, ndFloat32(1.0f)))); + return posit; } const ndQuaternion CalculateTargetRotation() const { const ndMatrix currentEffectorMatrix(m_effector->GetEffectorMatrix()); + const ndQuaternion currentRotation(currentEffectorMatrix); + const ndMatrix targetMatrix(ndPitchMatrix(m_pitch) * ndYawMatrix(m_yaw) * ndRollMatrix(m_roll)); - const ndQuaternion targetRotation(targetMatrix); - ndQuaternion currentRotation(currentEffectorMatrix); +#if 0 + ndQuaternion targetRotation(targetMatrix); +#else + ndQuaternion targetRotation(currentRotation); + ndFloat32 angleCos = currentEffectorMatrix.m_front.DotProduct(targetMatrix.m_front).GetScalar(); + if (angleCos < 0.999f) + { + ndFloat32 angle = ndAcos(angleCos); + ndVector pin(currentEffectorMatrix.m_front.CrossProduct(targetMatrix.m_front).Normalize()); + targetRotation = currentRotation * ndQuaternion(pin, angle); + //ndQuaternion targetRotation2(targetMatrix); + //ndQuaternion targetRotation3(targetMatrix); + //ndMatrix xxxx(ndCalculateMatrix(targetRotation)); + //ndMatrix xxxx1(targetMatrix * xxxx); + //ndMatrix xxxx2(targetMatrix * xxxx); + } +#endif if (currentRotation.DotProduct(targetRotation).GetScalar() < 0.0f) { - currentRotation = currentRotation.Scale(-1.0f); + targetRotation = targetRotation.Scale(-1.0f); } - ndQuaternion rotation(targetMatrix); - ndVector omega(currentRotation.CalcAverageOmega(targetRotation, 1.0f)); ndFloat32 omegaMag = ndSqrt(omega.DotProduct(omega).GetScalar()); ndFloat32 omegaSpeed = 5.0f * ndDegreeToRad; + + ndQuaternion rotation(targetRotation); if (omegaMag > omegaSpeed) { omega = omega.Normalize().Scale(omegaSpeed); @@ -267,9 +289,18 @@ namespace ndSimpleRobot m_world = world; m_timestep = timestep; + if (m_baseLimitJoint->GetJointHitLimits() || m_elbowLimitJoint->GetJointHitLimits()) + { + const ndMatrix currentEffectorMatrix(m_effector->GetEffectorMatrix()); + m_azimuth = -ndAtan2(currentEffectorMatrix.m_posit.m_y, currentEffectorMatrix.m_posit.m_z); + const ndVector localPosit(ndPitchMatrix(-m_azimuth).RotateVector(currentEffectorMatrix.m_posit) - m_effectorReference.m_posit); + m_x = localPosit.m_x; + m_z = localPosit.m_z; + } + m_leftGripper->SetOffsetPosit(-m_gripperPosit * 0.5f); m_rightGripper->SetOffsetPosit(-m_gripperPosit * 0.5f); - + const ndMatrix targetMatrix(CalculateNextTargetMatrix()); m_effector->SetOffsetMatrix(targetMatrix); @@ -295,12 +326,15 @@ namespace ndSimpleRobot ndBodyDynamic* m_rootBody; ndJointSlider* m_leftGripper; ndJointSlider* m_rightGripper; + ndJointHinge* m_baseLimitJoint; + ndJointHinge* m_elbowLimitJoint; ndIk6DofEffector* m_effector; + ndWorld* m_world; ndFixSizeArray m_backGround; ndReal m_x; - ndReal m_y; + ndReal m_z; ndReal m_azimuth; ndReal m_yaw; ndReal m_pitch; @@ -335,53 +369,58 @@ namespace ndSimpleRobot m_scene->Print(color, "Control panel"); ndInt8 change = 0; - change = change | ndInt8(ImGui::SliderFloat("x", &m_robot->m_x, ND_MIN_X_SPAND, ND_MAX_X_SPAND)); - change = change | ndInt8(ImGui::SliderFloat("y", &m_robot->m_y, ND_MIN_Y_SPAND, ND_MAX_Y_SPAND)); + change = change | ndInt8(ImGui::SliderFloat("y", &m_robot->m_x, ND_MIN_X_SPAND, ND_MAX_X_SPAND)); + change = change | ndInt8(ImGui::SliderFloat("z", &m_robot->m_z, ND_MIN_Z_SPAND, ND_MAX_Z_SPAND)); change = change | ndInt8(ImGui::SliderFloat("azimuth", &m_robot->m_azimuth, -ndPi, ndPi)); change = change | ndInt8(ImGui::SliderFloat("pitch", &m_robot->m_pitch, -ndPi, ndPi)); - change = change | ndInt8(ImGui::SliderFloat("yaw", &m_robot->m_yaw, -ndPi, ndPi)); - change = change | ndInt8(ImGui::SliderFloat("roll", &m_robot->m_roll, -ndPi * 0.35f, ndPi * 0.9f)); + change = change | ndInt8(ImGui::SliderFloat("yaw", &m_robot->m_yaw, -ndPi * 0.35f, ndPi * 0.8f)); + change = change | ndInt8(ImGui::SliderFloat("roll", &m_robot->m_roll, -ndPi, ndPi)); change = change | ndInt8(ImGui::SliderFloat("gripper", &m_robot->m_gripperPosit, -0.2f, 0.4f)); + //m_robot->m_azimuth = 60.0f * ndDegreeToRad; + bool newTarget = ndInt8(ImGui::Button("random target")); if (newTarget) { change = 1; m_robot->m_pitch = ndReal((2.0f * ndRand() - 1.0f) * ndPi); - m_robot->m_yaw = ndReal((2.0f * ndRand() - 1.0f) * ndPi); - m_robot->m_roll = ndReal(-ndPi * 0.35f + ndRand() * (ndPi * 0.9f - (-ndPi * 0.35f))); + m_robot->m_roll = ndReal((2.0f * ndRand() - 1.0f) * ndPi); + m_robot->m_yaw = ndReal(-ndPi * 0.35f + ndRand() * (ndPi * 0.8f - (-ndPi * 0.35f))); m_robot->m_azimuth = ndReal((2.0f * ndRand() - 1.0f) * ndPi); m_robot->m_x = ndReal(ND_MIN_X_SPAND + ndRand() * (ND_MAX_X_SPAND - ND_MIN_X_SPAND)); - m_robot->m_y = ndReal(ND_MIN_Y_SPAND + ndRand() * (ND_MAX_Y_SPAND - ND_MIN_Y_SPAND)); + m_robot->m_z = ndReal(ND_MIN_Z_SPAND + ndRand() * (ND_MAX_Z_SPAND - ND_MIN_Z_SPAND)); } bool newBoxTarget = ndInt8(ImGui::Button("random box target")); if (newBoxTarget) { change = 1; - + ndInt32 index = ndInt32 (ndRandInt() % 4); ndBodyKinematic* const body = m_robot->m_backGround[index]; const ndMatrix baseMatrix(m_robot->m_effector->CalculateGlobalBaseMatrix1()); - ndMatrix matrix(body->GetMatrix() * baseMatrix.OrthoInverse()); - matrix.m_posit.m_y += 0.75f; + ndMatrix bodyMatrix(ndGramSchmidtMatrix(ndVector(0.0f, 1.0f, 0.0f, 0.0f))); + bodyMatrix.m_posit = body->GetMatrix().m_posit; + ndMatrix matrix(bodyMatrix * baseMatrix.OrthoInverse()); + matrix.m_posit.m_x += 0.5f; + // intepolate in local space; - ndFloat32 azimuth = -ndAtan2(matrix.m_posit.m_z, matrix.m_posit.m_x); - const ndVector localPosit(ndYawMatrix(-azimuth).RotateVector(matrix.m_posit) - m_robot->m_effectorReference.m_posit); - + ndFloat32 azimuth = -ndAtan2(matrix.m_posit.m_y, matrix.m_posit.m_z); + const ndVector localPosit(ndPitchMatrix(-azimuth).RotateVector(matrix.m_posit) - m_robot->m_effectorReference.m_posit); + m_robot->m_azimuth = azimuth; m_robot->m_x = localPosit.m_x; - m_robot->m_y = localPosit.m_y; + m_robot->m_z = localPosit.m_z; ndVector euler1; ndVector euler(matrix.CalcPitchYawRoll(euler1)); - + + m_robot->m_yaw = euler.m_y; m_robot->m_roll = euler.m_z; m_robot->m_pitch = euler.m_x; - m_robot->m_yaw = euler.m_y + ndPi * 0.5f; } if (change) diff --git a/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.cpp b/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.cpp index dbe22c3db..86992c1f2 100644 --- a/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.cpp +++ b/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.cpp @@ -37,6 +37,7 @@ ndJointBilateralConstraint::ndJointBilateralConstraint() m_mark0 = 0; m_mark1 = 0; m_maxDof = 0; + m_hitLimits = 0; m_rowIsMotor = 0; m_isInSkeleton = 0; m_enableCollision = 0; @@ -71,11 +72,11 @@ ndJointBilateralConstraint::ndJointBilateralConstraint(ndInt32 maxDof, ndBodyKin m_mark0 = 0; m_mark1 = 0; - //m_maxDof = ndUnsigned32(maxDof); - m_maxDof = ndUnsigned8(maxDof); + m_hitLimits = 0; m_rowIsMotor = 0; m_isInSkeleton = 0; m_enableCollision = 0; + m_maxDof = ndUnsigned8(maxDof); m_solverModel = m_jointkinematicOpenLoop; m_defualtDiagonalRegularizer = ndFloat32(0.0f); @@ -110,11 +111,11 @@ ndJointBilateralConstraint::ndJointBilateralConstraint(ndInt32 maxDof, ndBodyKin m_mark0 = 0; m_mark1 = 0; - //m_maxDof = ndUnsigned32(maxDof); - m_maxDof = ndUnsigned8(maxDof); + m_hitLimits = 0; m_rowIsMotor = 0; m_isInSkeleton = 0; m_enableCollision = 0; + m_maxDof = ndUnsigned8(maxDof); m_solverModel = m_jointkinematicOpenLoop; m_defualtDiagonalRegularizer = ndFloat32(0.0f); @@ -132,6 +133,216 @@ ndJointBilateralConstraint::~ndJointBilateralConstraint() ndAssert(m_deletedNode == nullptr); } +ndJointBilateralSolverModel ndJointBilateralConstraint::GetSolverModel() const +{ + return m_solverModel; +} + +void ndJointBilateralConstraint::SetSolverModel(ndJointBilateralSolverModel model) +{ + ndAssert(model < m_jointModesCount); + ndAssert(model >= m_jointIterativeSoft); + m_solverModel = ndClamp(model, m_jointIterativeSoft, m_jointModesCount); +} + +//ndUnsigned32 ndJointBilateralConstraint::GetRowsCount() const +//{ +// return m_maxDof; +//} + +const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix0() const +{ + return m_localMatrix0; +} + +const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix1() const +{ + return m_localMatrix1; +} + +void ndJointBilateralConstraint::SetLocalMatrix0(const ndMatrix& matrix) +{ + m_localMatrix0 = matrix; +} + +void ndJointBilateralConstraint::SetLocalMatrix1(const ndMatrix& matrix) +{ + m_localMatrix1 = matrix; +} + + +ndFloat32 ndJointBilateralConstraint::GetMotorZeroAcceleration(ndConstraintDescritor& desc) const +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + return desc.m_zeroRowAcceleration[index]; +} + +void ndJointBilateralConstraint::SetMotorAcceleration(ndConstraintDescritor& desc, ndFloat32 acceleration) +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + m_rowIsMotor |= (1 << index); + desc.m_flags[index] = 0; + m_motorAcceleration[index] = acceleration; + desc.m_jointAccel[index] = acceleration; +} + +ndFloat32 ndJointBilateralConstraint::GetMotorAcceleration(ndConstraintDescritor& desc) const +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + return desc.m_jointAccel[index]; +} + +void ndJointBilateralConstraint::SetJointErrorPosit(ndConstraintDescritor& desc, ndFloat32 errorPosit) +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + desc.m_penetration[index] = errorPosit; +} + +void ndJointBilateralConstraint::SetLowerFriction(ndConstraintDescritor& desc, ndFloat32 friction) +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + desc.m_forceBounds[index].m_low = ndClamp(friction, ndFloat32(D_MIN_BOUND), ndFloat32(-0.001f)); + ndAssert(desc.m_forceBounds[index].m_normalIndex == D_INDEPENDENT_ROW); + +#ifdef _DEBUG + ndInt32 i0 = 0; + ndInt32 i1 = index - 1; + while ((i0 <= i1) && (desc.m_forceBounds[i0].m_normalIndex == D_INDEPENDENT_ROW)) i0++; + while ((i1 >= i0) && (desc.m_forceBounds[i1].m_normalIndex != D_INDEPENDENT_ROW)) i1--; + ndAssert((i0 - i1) == 1); + if ((i0 - i1) != 1) + { + ndTrace(("make sure that friction joint are issue at last\n")); + } +#endif +} + +void ndJointBilateralConstraint::SetHighFriction(ndConstraintDescritor& desc, ndFloat32 friction) +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + + desc.m_forceBounds[index].m_upper = ndClamp(friction, ndFloat32(0.001f), ndFloat32(D_MAX_BOUND)); + ndAssert(desc.m_forceBounds[index].m_normalIndex == D_INDEPENDENT_ROW); + +#ifdef _DEBUG + ndInt32 i0 = 0; + ndInt32 i1 = index - 1; + while ((i0 <= i1) && (desc.m_forceBounds[i0].m_normalIndex == D_INDEPENDENT_ROW)) i0++; + while ((i1 >= i0) && (desc.m_forceBounds[i1].m_normalIndex != D_INDEPENDENT_ROW)) i1--; + ndAssert((i0 - i1) == 1); + if ((i0 - i1) != 1) + { + ndTrace(("make sure that friction joint are issue at last\n")); + } +#endif +} + +void ndJointBilateralConstraint::JacobianDerivative(ndConstraintDescritor&) +{ + //ndAssert(0); + ndTrace(("error: this joint is an interface\n")); +} + +bool ndJointBilateralConstraint::GetSkeletonFlag() const +{ + return m_isInSkeleton ? true : false; +} + +void ndJointBilateralConstraint::SetSkeletonFlag(bool flag) +{ + m_isInSkeleton = ndUnsigned32(flag ? 1 : 0); +} + +bool ndJointBilateralConstraint::IsCollidable() const +{ + return m_enableCollision ? true : false; +} + +bool ndJointBilateralConstraint::IsBilateral() const +{ + return true; +} + +void ndJointBilateralConstraint::SetCollidable(bool state) +{ + m_enableCollision = ndUnsigned32(state ? 1 : 0); +} + +ndFloat32 ndJointBilateralConstraint::GetDiagonalRegularizer(const ndConstraintDescritor& desc) const +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + return desc.m_diagonalRegularizer[index]; +} + +void ndJointBilateralConstraint::SetDiagonalRegularizer(ndConstraintDescritor& desc, ndFloat32 regularizer) +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + desc.m_diagonalRegularizer[index] = ndClamp(regularizer, ndFloat32(0.0f), ndFloat32(1.0f)); +} + +ndFloat32 ndJointBilateralConstraint::GetJointErrorPosit(ndConstraintDescritor& desc) const +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + return desc.m_penetration[index]; +} + +ndFloat32 ndJointBilateralConstraint::GetJointErrorSpeed(ndConstraintDescritor& desc) const +{ + const ndInt32 index = desc.m_rowsCount - 1; + ndAssert(index >= 0); + ndAssert(index < ndInt32(m_maxDof)); + return desc.m_jointSpeed[index]; +} + +bool ndJointBilateralConstraint::IsInWorld() const +{ + return m_worldNode ? true : false; +} + +bool ndJointBilateralConstraint::IsSkeleton() const +{ + const ndJointBilateralSolverModel mode = GetSolverModel(); + bool test = false; + test = test || (mode == m_jointkinematicOpenLoop); + test = test || (mode == m_jointkinematicCloseLoop); + //test = test || (mode == m_jointkinematicHintOpenLoop); + return test; +} + +bool ndJointBilateralConstraint::GetJointHitLimits() const +{ + return m_hitLimits ? true : false; +} + +void ndJointBilateralConstraint::ReplaceSentinel(ndBodyKinematic* const sentinel) +{ + m_body1 = sentinel; +} + +ndJointBilateralConstraint* ndJointBilateralConstraint::GetAsBilateral() +{ + return this; +} + + void ndJointBilateralConstraint::SetIkMode(bool) { } diff --git a/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.h b/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.h index f77ecd50f..c2c8e8fc3 100644 --- a/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.h +++ b/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.h @@ -83,12 +83,10 @@ class ndJointBilateralConstraint : public ndConstraint D_COLLISION_API ndJointBilateralConstraint(ndInt32 maxDof, ndBodyKinematic* const body0, ndBodyKinematic* const body1, const ndMatrix& globalMatrixBody0, const ndMatrix& globalMatrixBody1); D_COLLISION_API virtual ~ndJointBilateralConstraint(); - void ReplaceSentinel(ndBodyKinematic* const sentinel); - - virtual ndJointBilateralConstraint* GetAsBilateral(); - virtual void JacobianDerivative(ndConstraintDescritor& desc); - virtual ndJointBilateralSolverModel GetSolverModel() const; - virtual void SetSolverModel(ndJointBilateralSolverModel model); + D_COLLISION_API virtual ndJointBilateralConstraint* GetAsBilateral(); + D_COLLISION_API virtual void JacobianDerivative(ndConstraintDescritor& desc); + D_COLLISION_API virtual ndJointBilateralSolverModel GetSolverModel() const; + D_COLLISION_API virtual void SetSolverModel(ndJointBilateralSolverModel model); D_COLLISION_API ndFloat32 CalculateAngle(const ndVector& planeDir, const ndVector& cosDir, const ndVector& sinDir) const; D_COLLISION_API virtual void JointAccelerations(ndJointAccelerationDecriptor* const desc); @@ -101,37 +99,40 @@ class ndJointBilateralConstraint : public ndConstraint D_COLLISION_API virtual ndInt32 GetKinematicState(ndKinematicState* const state) const; - const ndMatrix& GetLocalMatrix0() const; - const ndMatrix& GetLocalMatrix1() const; + D_COLLISION_API const ndMatrix& GetLocalMatrix0() const; + D_COLLISION_API const ndMatrix& GetLocalMatrix1() const; + D_COLLISION_API void SetLocalMatrix0(const ndMatrix& matrix); + D_COLLISION_API void SetLocalMatrix1(const ndMatrix& matrix); D_COLLISION_API virtual ndMatrix CalculateGlobalMatrix0() const; D_COLLISION_API virtual ndMatrix CalculateGlobalMatrix1() const; + D_COLLISION_API virtual void CalculateGlobalMatrix(ndMatrix& matrix0, ndMatrix& matrix1) const; D_COLLISION_API void CalculateLocalMatrix(const ndMatrix& pinsAndPivotFrame, ndMatrix& localMatrix0, ndMatrix& localMatrix1) const; - void SetLocalMatrix0(const ndMatrix& matrix); - void SetLocalMatrix1(const ndMatrix& matrix); + D_COLLISION_API bool IsInWorld() const; + D_COLLISION_API bool IsSkeleton() const; + D_COLLISION_API bool IsBilateral() const; + D_COLLISION_API bool IsCollidable() const; + D_COLLISION_API bool GetSkeletonFlag() const; + D_COLLISION_API void SetCollidable(bool state); + D_COLLISION_API void SetSkeletonFlag(bool flag); + + D_COLLISION_API bool GetJointHitLimits() const; - bool IsInWorld() const; - bool IsSkeleton() const; - bool IsBilateral() const; - bool IsCollidable() const; - bool GetSkeletonFlag() const; - void SetCollidable(bool state); - void SetSkeletonFlag(bool flag); - virtual bool IsHolonomic(ndFloat32 timestep) const; + D_COLLISION_API void SetHighFriction(ndConstraintDescritor& desc, ndFloat32 friction); + D_COLLISION_API void SetLowerFriction(ndConstraintDescritor& desc, ndFloat32 friction); + D_COLLISION_API void SetJointErrorPosit(ndConstraintDescritor& desc, ndFloat32 errorPosit); + D_COLLISION_API void SetMotorAcceleration(ndConstraintDescritor& desc, ndFloat32 acceleration); + D_COLLISION_API void SetDiagonalRegularizer(ndConstraintDescritor& desc, ndFloat32 regularizer); - void SetHighFriction(ndConstraintDescritor& desc, ndFloat32 friction); - void SetLowerFriction(ndConstraintDescritor& desc, ndFloat32 friction); - void SetJointErrorPosit(ndConstraintDescritor& desc, ndFloat32 errorPosit); - void SetMotorAcceleration(ndConstraintDescritor& desc, ndFloat32 acceleration); - void SetDiagonalRegularizer(ndConstraintDescritor& desc, ndFloat32 regularizer); + D_COLLISION_API ndFloat32 GetJointErrorPosit(ndConstraintDescritor& desc) const; + D_COLLISION_API ndFloat32 GetJointErrorSpeed(ndConstraintDescritor& desc) const; + D_COLLISION_API ndFloat32 GetMotorAcceleration(ndConstraintDescritor& desc) const; + D_COLLISION_API ndFloat32 GetMotorZeroAcceleration(ndConstraintDescritor& desc) const; + D_COLLISION_API ndFloat32 GetDiagonalRegularizer(const ndConstraintDescritor& desc) const; - ndFloat32 GetJointErrorPosit(ndConstraintDescritor& desc) const; - ndFloat32 GetJointErrorSpeed(ndConstraintDescritor& desc) const; - ndFloat32 GetMotorAcceleration(ndConstraintDescritor& desc) const; - ndFloat32 GetMotorZeroAcceleration(ndConstraintDescritor& desc) const; - ndFloat32 GetDiagonalRegularizer(const ndConstraintDescritor& desc) const; + D_COLLISION_API void ReplaceSentinel(ndBodyKinematic* const sentinel); protected: // inverse dynamics interface @@ -157,6 +158,7 @@ class ndJointBilateralConstraint : public ndConstraint ndUnsigned32 m_isInSkeleton : 1; ndUnsigned32 m_enableCollision : 1; ndInt8 m_rowIsMotor; + ndInt8 m_hitLimits; ndJointBilateralSolverModel m_solverModel; friend class ndWorld; @@ -170,213 +172,6 @@ class ndJointBilateralConstraint : public ndConstraint friend class ndDynamicsUpdateCuda; }; -inline ndJointBilateralSolverModel ndJointBilateralConstraint::GetSolverModel() const -{ - return m_solverModel; -} - -inline void ndJointBilateralConstraint::SetSolverModel(ndJointBilateralSolverModel model) -{ - ndAssert(model < m_jointModesCount); - ndAssert(model >= m_jointIterativeSoft); - m_solverModel = ndClamp(model, m_jointIterativeSoft, m_jointModesCount); -} - -//inline ndUnsigned32 ndJointBilateralConstraint::GetRowsCount() const -//{ -// return m_maxDof; -//} - -inline const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix0() const -{ - return m_localMatrix0; -} - -inline const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix1() const -{ - return m_localMatrix1; -} - -inline void ndJointBilateralConstraint::SetLocalMatrix0(const ndMatrix& matrix) -{ - m_localMatrix0 = matrix; -} - -inline void ndJointBilateralConstraint::SetLocalMatrix1(const ndMatrix& matrix) -{ - m_localMatrix1 = matrix; -} - - -inline ndFloat32 ndJointBilateralConstraint::GetMotorZeroAcceleration(ndConstraintDescritor& desc) const -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - return desc.m_zeroRowAcceleration[index]; -} - -inline void ndJointBilateralConstraint::SetMotorAcceleration(ndConstraintDescritor& desc, ndFloat32 acceleration) -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - m_rowIsMotor |= (1 << index); - desc.m_flags[index] = 0; - m_motorAcceleration[index] = acceleration; - desc.m_jointAccel[index] = acceleration; -} - -inline ndFloat32 ndJointBilateralConstraint::GetMotorAcceleration(ndConstraintDescritor& desc) const -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - return desc.m_jointAccel[index]; -} - -inline void ndJointBilateralConstraint::SetJointErrorPosit(ndConstraintDescritor& desc, ndFloat32 errorPosit) -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - desc.m_penetration[index] = errorPosit; -} - -inline void ndJointBilateralConstraint::SetLowerFriction(ndConstraintDescritor& desc, ndFloat32 friction) -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32 (m_maxDof)); - desc.m_forceBounds[index].m_low = ndClamp(friction, ndFloat32(D_MIN_BOUND), ndFloat32(-0.001f)); - ndAssert(desc.m_forceBounds[index].m_normalIndex == D_INDEPENDENT_ROW); - - #ifdef _DEBUG - ndInt32 i0 = 0; - ndInt32 i1 = index - 1; - while ((i0 <= i1) && (desc.m_forceBounds[i0].m_normalIndex == D_INDEPENDENT_ROW)) i0++; - while ((i1 >= i0) && (desc.m_forceBounds[i1].m_normalIndex != D_INDEPENDENT_ROW)) i1--; - ndAssert((i0 - i1) == 1); - if ((i0 - i1) != 1) - { - ndTrace(("make sure that friction joint are issue at last\n")); - } - #endif -} - -inline void ndJointBilateralConstraint::SetHighFriction(ndConstraintDescritor& desc, ndFloat32 friction) -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - - desc.m_forceBounds[index].m_upper = ndClamp(friction, ndFloat32(0.001f), ndFloat32(D_MAX_BOUND)); - ndAssert(desc.m_forceBounds[index].m_normalIndex == D_INDEPENDENT_ROW); - - #ifdef _DEBUG - ndInt32 i0 = 0; - ndInt32 i1 = index - 1; - while ((i0 <= i1) && (desc.m_forceBounds[i0].m_normalIndex == D_INDEPENDENT_ROW)) i0++; - while ((i1 >= i0) && (desc.m_forceBounds[i1].m_normalIndex != D_INDEPENDENT_ROW)) i1--; - ndAssert((i0 - i1) == 1); - if ((i0 - i1) != 1) - { - ndTrace(("make sure that friction joint are issue at last\n")); - } - #endif -} -inline void ndJointBilateralConstraint::JacobianDerivative(ndConstraintDescritor&) -{ - //ndAssert(0); - ndTrace(("error: this joint is an interface\n")); -} - -inline bool ndJointBilateralConstraint::GetSkeletonFlag() const -{ - return m_isInSkeleton ? true : false; -} - -inline void ndJointBilateralConstraint::SetSkeletonFlag(bool flag) -{ - m_isInSkeleton = ndUnsigned32 (flag ? 1 : 0); -} - -inline bool ndJointBilateralConstraint::IsCollidable() const -{ - return m_enableCollision ? true : false; -} - -inline bool ndJointBilateralConstraint::IsBilateral() const -{ - return true; -} - -inline void ndJointBilateralConstraint::SetCollidable(bool state) -{ - m_enableCollision = ndUnsigned32 (state ? 1 : 0); -} - -inline ndFloat32 ndJointBilateralConstraint::GetDiagonalRegularizer(const ndConstraintDescritor& desc) const -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - return desc.m_diagonalRegularizer[index]; -} - -inline void ndJointBilateralConstraint::SetDiagonalRegularizer(ndConstraintDescritor& desc, ndFloat32 regularizer) -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - desc.m_diagonalRegularizer[index] = ndClamp(regularizer, ndFloat32(0.0f), ndFloat32(1.0f)); -} - -inline ndFloat32 ndJointBilateralConstraint::GetJointErrorPosit(ndConstraintDescritor& desc) const -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - return desc.m_penetration[index]; -} - -inline ndFloat32 ndJointBilateralConstraint::GetJointErrorSpeed(ndConstraintDescritor& desc) const -{ - const ndInt32 index = desc.m_rowsCount - 1; - ndAssert(index >= 0); - ndAssert(index < ndInt32(m_maxDof)); - return desc.m_jointSpeed[index]; -} - -inline bool ndJointBilateralConstraint::IsInWorld() const -{ - return m_worldNode ? true : false; -} - -inline bool ndJointBilateralConstraint::IsSkeleton() const -{ - const ndJointBilateralSolverModel mode = GetSolverModel(); - bool test = false; - test = test || (mode == m_jointkinematicOpenLoop); - test = test || (mode == m_jointkinematicCloseLoop); - //test = test || (mode == m_jointkinematicHintOpenLoop); - return test; -} - -inline void ndJointBilateralConstraint::ReplaceSentinel(ndBodyKinematic* const sentinel) -{ - m_body1 = sentinel; -} - -inline ndJointBilateralConstraint* ndJointBilateralConstraint::GetAsBilateral() -{ - return this; -} - -inline bool ndJointBilateralConstraint::IsHolonomic(ndFloat32) const -{ - return true; -} #endif diff --git a/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.cpp b/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.cpp index 779115442..08d8a4c42 100644 --- a/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.cpp +++ b/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.cpp @@ -316,25 +316,6 @@ void ndIk6DofEffector::SubmitLinearAxis(const ndMatrix& matrix0, const ndMatrix& #endif } -bool ndIk6DofEffector::IsHolonomic(ndFloat32 timestep) const -{ - ndAssert(m_body0->GetSkeleton()); - bool isHolonomic = false; - const ndSkeletonContainer* const skeleton = m_body0->GetSkeleton(); - if (skeleton) - { - const ndSkeletonContainer::ndNode* const effectorNode = skeleton->GetNodeList().FindNode(m_body0); - ndAssert(effectorNode); - - isHolonomic = true; - for (const ndSkeletonContainer::ndNode* node = effectorNode; isHolonomic && node && (node->m_body != m_body1); node = node->m_parent) - { - isHolonomic = isHolonomic && node->m_joint->IsHolonomic(timestep); - } - } - return isHolonomic; -} - void ndIk6DofEffector::JacobianDerivative(ndConstraintDescritor& desc) { ndMatrix matrix0; diff --git a/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.h b/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.h index 2735c3b08..6934ee43b 100644 --- a/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.h +++ b/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.h @@ -60,7 +60,6 @@ class ndIk6DofEffector: public ndJointBilateralConstraint D_NEWTON_API ndMatrix CalculateGlobalMatrix1() const; D_NEWTON_API ndMatrix CalculateGlobalBaseMatrix1() const; - D_NEWTON_API virtual bool IsHolonomic(ndFloat32 timestep) const; protected: D_NEWTON_API void JacobianDerivative(ndConstraintDescritor& desc); diff --git a/newton-4.00/sdk/dNewton/dIkSolver/ndIkJointHinge.cpp b/newton-4.00/sdk/dNewton/dIkSolver/ndIkJointHinge.cpp index 287e6920d..dc62e1d48 100644 --- a/newton-4.00/sdk/dNewton/dIkSolver/ndIkJointHinge.cpp +++ b/newton-4.00/sdk/dNewton/dIkSolver/ndIkJointHinge.cpp @@ -48,31 +48,6 @@ void ndIkJointHinge::SetMaxTorque(ndFloat32 maxTorque) m_maxTorque = ndAbs(maxTorque); } -bool ndIkJointHinge::IsHolonomic(ndFloat32 timestep) const -{ - if (m_limitState) - { - if ((m_minLimit > (ndFloat32(-1.0f) * ndDegreeToRad)) && (m_maxLimit < (ndFloat32(1.0f) * ndDegreeToRad))) - { - ndAssert(0); - return false; - } - else - { - const ndFloat32 angle = m_angle + m_omega * timestep; - if (angle < m_minLimit) - { - return false; - } - else if (angle > m_maxLimit) - { - return false; - } - } - } - return true; -} - void ndIkJointHinge::JacobianDerivative(ndConstraintDescritor& desc) { ndMatrix matrix0; diff --git a/newton-4.00/sdk/dNewton/dIkSolver/ndIkJointHinge.h b/newton-4.00/sdk/dNewton/dIkSolver/ndIkJointHinge.h index ac3c98be4..8a2e81c34 100644 --- a/newton-4.00/sdk/dNewton/dIkSolver/ndIkJointHinge.h +++ b/newton-4.00/sdk/dNewton/dIkSolver/ndIkJointHinge.h @@ -35,7 +35,6 @@ class ndIkJointHinge: public ndJointHinge, public ndJointBilateralConstraint::nd D_NEWTON_API void SetMaxTorque(ndFloat32 maxTorque); protected: - D_NEWTON_API bool IsHolonomic(ndFloat32 timestep) const; D_NEWTON_API void JacobianDerivative(ndConstraintDescritor& desc); ndFloat32 m_maxTorque; diff --git a/newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.cpp b/newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.cpp index b4ce3ef86..6b4f39f92 100644 --- a/newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.cpp +++ b/newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.cpp @@ -255,26 +255,6 @@ ndInt32 ndIkSwivelPositionEffector::GetKinematicState(ndKinematicState* const st return 4; } -bool ndIkSwivelPositionEffector::IsHolonomic(ndFloat32 timestep) const -{ - ndAssert(m_body0->GetSkeleton()); - - bool isHolonomic = true; - const ndSkeletonContainer* const skeleton = m_body0->GetSkeleton(); - if (skeleton) - { - const ndSkeletonContainer::ndNode* const effectorNode = skeleton->GetNodeList().FindNode(m_body0); - ndAssert(effectorNode); - - isHolonomic = true; - for (const ndSkeletonContainer::ndNode* node = effectorNode; isHolonomic && node && (node->m_body != m_body1); node = node->m_parent) - { - isHolonomic = isHolonomic && node->m_joint->IsHolonomic(timestep); - } - } - return isHolonomic; -} - ndFloat32 ndIkSwivelPositionEffector::CalculateLookAtSwivelAngle(const ndVector& upDir) const { ndFloat32 swivelAngle = GetSwivelAngle(); diff --git a/newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.h b/newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.h index 3a6e58444..db6dafb18 100644 --- a/newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.h +++ b/newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.h @@ -58,8 +58,6 @@ class ndIkSwivelPositionEffector: public ndJointBilateralConstraint D_NEWTON_API void DebugJoint(ndConstraintDebugCallback& debugCallback) const; D_NEWTON_API ndInt32 GetKinematicState(ndKinematicState* const state) const; - D_NEWTON_API virtual bool IsHolonomic(ndFloat32 timestep) const; - protected: ndMatrix CalculateSwivelFrame(const ndMatrix& matrix1) const; D_NEWTON_API void JacobianDerivative(ndConstraintDescritor& desc); diff --git a/newton-4.00/sdk/dNewton/dJoints/ndJointHinge.cpp b/newton-4.00/sdk/dNewton/dJoints/ndJointHinge.cpp index 80e4c2247..3750b93c5 100644 --- a/newton-4.00/sdk/dNewton/dJoints/ndJointHinge.cpp +++ b/newton-4.00/sdk/dNewton/dJoints/ndJointHinge.cpp @@ -253,6 +253,7 @@ ndInt32 ndJointHinge::GetKinematicState(ndKinematicState* const state) const void ndJointHinge::SubmitLimits(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1) { + m_hitLimits = false; if (m_limitState) { if ((m_minLimit > (ndFloat32(-1.0f) * ndDegreeToRad)) && (m_maxLimit < (ndFloat32(1.0f) * ndDegreeToRad))) @@ -264,6 +265,7 @@ void ndJointHinge::SubmitLimits(ndConstraintDescritor& desc, const ndMatrix& mat const ndFloat32 angle = m_angle + m_omega * desc.m_timestep; if (angle < m_minLimit) { + m_hitLimits = true; AddAngularRowJacobian(desc, &matrix0.m_front[0], ndFloat32(0.0f)); const ndFloat32 stopAccel = GetMotorZeroAcceleration(desc); const ndFloat32 penetration = angle - m_minLimit; @@ -273,6 +275,7 @@ void ndJointHinge::SubmitLimits(ndConstraintDescritor& desc, const ndMatrix& mat } else if (angle > m_maxLimit) { + m_hitLimits = true; AddAngularRowJacobian(desc, &matrix0.m_front[0], ndFloat32(0.0f)); const ndFloat32 stopAccel = GetMotorZeroAcceleration(desc); const ndFloat32 penetration = angle - m_maxLimit;