void dCustomCorkScrew::SetAngularSpringDamper(bool state, dFloat springDamperRelaxation, dFloat spring, dFloat damper)
{
m_angularSpring = spring;
m_angularDamper = damper;
m_options.m_option3 = state;
m_angularSpringDamperRelaxation = dClamp(springDamperRelaxation, dFloat(0.0f), dFloat(0.999f));
}
void CustomPlayerController::SetPlayerOrigin (dFloat originHeight)
{
/*
NewtonCollision* const playerShape = NewtonBodyGetCollision(m_body);
NewtonCompoundCollisionBeginAddRemove(playerShape);
dMatrix supportShapeMatrix (dGetIdentityMatrix());
supportShapeMatrix[0] = m_upVector;
supportShapeMatrix[1] = m_frontVector;
supportShapeMatrix[2] = supportShapeMatrix[0] * supportShapeMatrix[1];
supportShapeMatrix.m_posit = supportShapeMatrix[0].Scale(m_height * 0.5f - originHigh);
supportShapeMatrix.m_posit.m_w = 1.0f;
NewtonCollisionSetMatrix (m_supportShape, &supportShapeMatrix[0][0]);
dMatrix collisionShapeMatrix (supportShapeMatrix);
dFloat cylinderHeight = m_height - m_stairStep;
dAssert (cylinderHeight > 0.0f);
collisionShapeMatrix.m_posit = collisionShapeMatrix[0].Scale(cylinderHeight * 0.5f + m_stairStep - originHigh);
collisionShapeMatrix.m_posit.m_w = 1.0f;
NewtonCollisionSetMatrix (m_upperBodyShape, &collisionShapeMatrix[0][0]);
NewtonCompoundCollisionEndAddRemove (playerShape);
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodySetMassProperties(m_body, mass, playerShape);
*/
originHeight = dClamp (originHeight, dFloat(0.0f), m_height);
dVector origin (m_upVector.Scale (originHeight));
NewtonBodySetCentreOfMass (m_body, &origin[0]);
}
void dCustomHinge::SetAsSpringDamper(bool state, dFloat springDamperRelaxation, dFloat spring, dFloat damper)
{
m_setAsSpringDamper = state;
m_spring = spring;
m_damper = damper;
m_springDamperRelaxation = dClamp(springDamperRelaxation, dFloat(0.0f), dFloat(0.999f));
}
dBigVector dBezierSpline::CurveDerivative (dFloat64 u, int index) const
{
u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
dAssert (index <= m_degree);
dFloat64 basicsFuncDerivatives[D_BEZIER_LOCAL_BUFFER_SIZE];
int span = GetSpan(u);
BasicsFunctionsDerivatives (u, span, basicsFuncDerivatives);
const int with = m_degree + 1;
dBigVector point (0.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; i <= m_degree; i ++) {
point += m_controlPoints[span - m_degree + i].Scale (basicsFuncDerivatives[with * index + i]);
}
return point;
}
void dPluginCamera::SetMatrix (dFloat yaw, dFloat roll, dFloat distance)
{
// dMatrix yawMatrix_ (dYawMatrix(yaw));
// dMatrix rollMatrix_ (dRollMatrix(roll));
// m_matrix = rollMatrix_ * yawMatrix_;
// m_matrix.m_posit = dVector (-10.0f, 5.0f, 10.0f, 1.0f);
m_cameraRoll = dClamp(roll, -85.0f * 3.141692f / 180.0f, 85.0f * 3.141692f / 180.0f);
m_cameraYaw = dMod (yaw, 2.0f * 3.141592f);
m_distance = (distance < 0.5f) ? 0.5f : distance;
dMatrix yawMatrix (dYawMatrix(m_cameraYaw));
dMatrix rollMatrix (dRollMatrix(m_cameraRoll));
m_matrix = rollMatrix * yawMatrix;
m_matrix.m_posit = m_matrix.RotateVector(dVector (-distance, 0.0f, 0.0f, 1.0f)) + m_pointOfInterest;
m_matrix.m_posit.m_w = 1.0f;
}
dFloat CustomVehicleControllerComponent::dInterpolationCurve::GetValue (dFloat param) const
{
dFloat interplatedValue = 0.0f;
if (m_count) {
param = dClamp (param, 0.0f, m_nodes[m_count - 1].m_param);
interplatedValue = m_nodes[m_count - 1].m_value;
for (int i = 1; i < m_count; i ++) {
if (param < m_nodes[i].m_param) {
dFloat df = m_nodes[i].m_value - m_nodes[i - 1].m_value;
dFloat ds = m_nodes[i].m_param - m_nodes[i - 1].m_param;
dFloat step = param - m_nodes[i - 1].m_param;
interplatedValue = m_nodes[i - 1].m_value + df * step / ds;
break;
}
}
}
return interplatedValue;
}
void dCustomBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
SubmitLinearRows(0x07, matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0);
dMatrix coneRotation(dGetIdentityMatrix());
dVector lateralDir(matrix0.m_up);
if (cosAngleCos < 0.9999f) {
lateralDir = coneDir1.CrossProduct(coneDir0);
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale (-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
}
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
dVector relOmega(omega0 - omega1);
// do twist angle calculations
dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse());
dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1]));
if (m_options.m_option0) {
if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) {
NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else {
SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep);
}
}
} else if (m_options.m_option1) {
// TODO spring option
dAssert (0);
} else if (m_twistFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
}
// do twist cone angle calculations
if (m_options.m_option2) {
if ((m_maxConeAngle == 0.0f)) {
dMatrix localMatrix(matrix0 * matrix1.Inverse());
dVector euler0;
dVector euler1;
localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
} else {
if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else {
dFloat jointOmega = relOmega.DotProduct3(lateralDir);
dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)));
dFloat coneAngle = currentAngle + jointOmega * timestep;
if (coneAngle >= m_maxConeAngle) {
//dQuaternion rot(lateralDir, coneAngle);
//dVector frontDir(rot.RotateVector(coneDir1));
//dVector upDir(lateralDir.CrossProduct(frontDir));
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
const dFloat invtimestep = 1.0f / timestep;
const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep;
const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
NewtonUserJointSetRowAcceleration(m_joint, stopAccel);
} else if (m_coneFriction != 0) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
}
} else if (m_options.m_option3) {
// TODO spring option
dAssert(0);
} else if (m_coneFriction > 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
dVector upDir(lateralDir.CrossProduct(coneDir0));
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
}
}
void dCustomBallAndSocket::Debug(dDebugDisplay* const debugDisplay) const
{
dMatrix matrix0;
dMatrix matrix1;
dCustomJoint::Debug(debugDisplay);
CalculateGlobalMatrix(matrix0, matrix1);
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngleCos = coneDir0.DotProduct3(coneDir1);
dMatrix coneRotation(dGetIdentityMatrix());
if (cosAngleCos < 0.9999f) {
dVector lateralDir(coneDir1.CrossProduct(coneDir0));
dFloat mag2 = lateralDir.DotProduct3(lateralDir);
//dAssert(mag2 > 1.0e-4f);
if (mag2 > 1.0e-4f) {
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
} else {
lateralDir = matrix0.m_up.Scale(-1.0f);
coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
}
} else if (cosAngleCos < -0.9999f) {
coneRotation[0][0] = -1.0f;
coneRotation[1][1] = -1.0f;
}
const int subdiv = 18;
const dFloat radius = debugDisplay->m_debugScale;
dVector arch[subdiv + 1];
// show twist angle limits
if (m_options.m_option0 && ((m_maxTwistAngle - m_minTwistAngle) > dFloat(1.0e-3f))) {
dMatrix pitchMatrix(matrix1 * coneRotation);
pitchMatrix.m_posit = matrix1.m_posit;
dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f));
dFloat angleStep = dMin (m_maxTwistAngle - m_minTwistAngle, dFloat (2.0f * dPi)) / subdiv;
dFloat angle0 = m_minTwistAngle;
debugDisplay->SetColor(dVector(0.6f, 0.2f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
arch[i] = pitchMatrix.TransformVector(dPitchMatrix(angle0).RotateVector(point));
debugDisplay->DrawLine(pitchMatrix.m_posit, arch[i]);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
// show cone angle limits
if (m_options.m_option2) {
dVector point(radius * dCos(m_maxConeAngle), radius * dSin(m_maxConeAngle), 0.0f, 0.0f);
dFloat angleStep = dPi * 2.0f / subdiv;
dFloat angle0 = 0.0f;
debugDisplay->SetColor(dVector(0.3f, 0.8f, 0.0f, 0.0f));
for (int i = 0; i <= subdiv; i++) {
dVector conePoint(dPitchMatrix(angle0).RotateVector(point));
dVector p(matrix1.TransformVector(conePoint));
arch[i] = p;
debugDisplay->DrawLine(matrix1.m_posit, p);
angle0 += angleStep;
}
for (int i = 0; i < subdiv; i++) {
debugDisplay->DrawLine(arch[i], arch[i + 1]);
}
}
}
void NewtonDemos::OnSelectBroadPhase(wxCommandEvent& event)
{
BEGIN_MENU_OPTION();
m_broadPhaseType = dClamp (event.GetId() - ID_BROADPHSE_TYPE0, 0, 1);
END_MENU_OPTION();
}
void DemoCameraListener::PreUpdate (const NewtonWorld* const world, dFloat timestep)
{
// update the camera;
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
dMatrix targetMatrix (m_camera->GetNextMatrix());
int mouseX;
int mouseY;
scene->GetMousePosition (mouseX, mouseY);
// slow down the Camera if we have a Body
dFloat slowDownFactor = scene->IsShiftKeyDown() ? 0.5f/10.0f : 0.5f;
// do camera translation
if (scene->GetKeyState ('W')) {
targetMatrix.m_posit += targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('S')) {
targetMatrix.m_posit -= targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('A')) {
targetMatrix.m_posit -= targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('D')) {
targetMatrix.m_posit += targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('Q')) {
targetMatrix.m_posit -= targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('E')) {
targetMatrix.m_posit += targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
// do camera rotation, only if we do not have anything picked
bool buttonState = m_mouseLockState || scene->GetMouseKeyState(0);
if (!m_targetPicked && buttonState) {
int mouseSpeedX = mouseX - m_mousePosX;
int mouseSpeedY = mouseY - m_mousePosY;
if (mouseSpeedX > 0) {
m_yaw = dMod(m_yaw + m_yawRate, 2.0f * 3.1416f);
} else if (mouseSpeedX < 0){
m_yaw = dMod(m_yaw - m_yawRate, 2.0f * 3.1416f);
}
if (mouseSpeedY > 0) {
m_pitch += m_pitchRate;
} else if (mouseSpeedY < 0){
m_pitch -= m_pitchRate;
}
m_pitch = dClamp(m_pitch, dFloat (-80.0f * 3.1416f / 180.0f), dFloat (80.0f * 3.1416f / 180.0f));
}
m_mousePosX = mouseX;
m_mousePosY = mouseY;
dMatrix matrix (dRollMatrix(m_pitch) * dYawMatrix(m_yaw));
dQuaternion rot (matrix);
m_camera->SetMatrix (*scene, rot, targetMatrix.m_posit);
UpdatePickBody(scene, timestep);
}
dFloat dNewton::GetInterpolationParam(dFloat timestepInSecunds) const
{
dLong timeStep = GetTimeInMicrosenconds () - m_microseconds;
dFloat param = (dFloat (timeStep) * 1.0e-6f) / timestepInSecunds;
return dClamp (param, dFloat(0.0f), dFloat(1.0f));
}
void dCustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{
// check if this is an impulsive time step
dMatrix matrix0(GetBodyMatrix());
dVector omega(0.0f);
dVector com(0.0f);
dVector pointVeloc(0.0f);
const dFloat damp = 0.3f;
dAssert (timestep > 0.0f);
const dFloat invTimestep = 1.0f / timestep;
// we not longer cap excessive angular velocities, it is left to the client application.
NewtonBodyGetOmega(m_body0, &omega[0]);
//cap excessive angular velocities
dFloat mag2 = omega.DotProduct3(omega);
if (mag2 > (m_omegaCap * m_omegaCap)) {
omega = omega.Normalize().Scale(m_omegaCap);
NewtonBodySetOmega(m_body0, &omega[0]);
}
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
dVector relPosit(m_targetMatrix.m_posit - matrix0.m_posit);
NewtonBodyGetPointVelocity(m_body0, &m_targetMatrix.m_posit[0], &pointVeloc[0]);
for (int i = 0; i < 3; i ++) {
// Restrict the movement on the pivot point along all tree orthonormal direction
dFloat speed = pointVeloc.DotProduct3(m_targetMatrix[i]);
dFloat dist = relPosit.DotProduct3(m_targetMatrix[i]) * damp;
dFloat relSpeed = dist * invTimestep - speed;
dFloat relAccel = relSpeed * invTimestep;
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &m_targetMatrix[i][0]);
NewtonUserJointSetRowAcceleration(m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxLinearFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_maxLinearFriction);
}
if (m_isSixdof) {
dQuaternion rotation (matrix0.Inverse() * m_targetMatrix);
if (dAbs (rotation.m_q0) < 0.99998f) {
dMatrix rot (dGrammSchmidt(dVector (rotation.m_q1, rotation.m_q2, rotation.m_q3)));
dFloat angle = 2.0f * dAcos(dClamp(rotation.m_q0, dFloat(-1.0f), dFloat(1.0f)));
NewtonUserJointAddAngularRow (m_joint, angle, &rot.m_front[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_up[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_right[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
} else {
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction);
}
}
}
void CustomUniversalActuator::SetTargetAngle1(dFloat angle)
{
m_angle1 = dClamp (angle, m_minAngle1, m_maxAngle1);
}
dFloat64 dBezierSpline::FindClosestKnot (dBigVector& closestPoint, const dBigVector& point, int subdivitionSteps) const
{
int startSpan = m_degree;
dFloat64 bestU = 0.0f;
dFloat64 distance2 = 1.0e10f;
dBigVector closestControlPoint (m_controlPoints[0]);
subdivitionSteps = dMax (subdivitionSteps, 1);
dFloat64 scale = 1.0f / subdivitionSteps;
for (int span = m_degree; span < (m_knotsCount - m_degree - 1); span ++) {
dFloat64 param = 0.0f;
for (int i = 0; i < subdivitionSteps; i ++) {
dFloat64 u = m_knotVector[span] + (m_knotVector[span + 1] - m_knotVector[span]) * param;
param += scale;
dBigVector p (CurvePoint (u, span));
dBigVector dp (p - point);
dFloat64 dist2 = dp.DotProduct3(dp);
if (dist2 < distance2) {
bestU = u;
startSpan = span;
distance2 = dist2;
closestControlPoint = p;
}
}
}
dBigVector derivatives[32];
dFloat64 u0 = bestU;
bool stop = false;
for (int i = 0; (i < 20) && !stop; i ++) {
CurveAllDerivatives (u0, derivatives);
dBigVector dist (closestControlPoint - point);
dFloat64 num = derivatives[1].DotProduct3(dist);
dFloat64 den = derivatives[2].DotProduct3(dist) + derivatives[1].DotProduct3(derivatives[1]);
// if (dAbs (den) < 1.0e-6f)
// __debugbreak();
dFloat64 u1 = dClamp(u0 - num / den, dFloat64(0.0), dFloat64 (1.0));
if (u1 < m_knotVector[startSpan]) {
startSpan --;
dAssert (startSpan >= 0);
} else if (u1 >= m_knotVector[startSpan + 1]) {
startSpan ++;
dAssert (startSpan < (m_knotsCount - m_degree));
}
dAssert (startSpan >= m_degree);
dAssert (startSpan <= (m_knotsCount - m_degree - 1));
closestControlPoint = CurvePoint (u1, startSpan);
//dFloat64 xxx0 = num * num;
//dFloat64 xxx1 = dist % dist;
//dFloat64 xxx2 = derivatives[1] % derivatives[1];
//dFloat64 xxx3 = xxx1 * xxx2 * 1.0e-10;
stop |= (dAbs (u1 - u0) < 1.0e-10) || (num * num < ((dist.DotProduct3(dist)) * (derivatives[1].DotProduct3(derivatives[1])) * 1.0e-10));
u0 = u1;
}
closestPoint = closestControlPoint;
return u0;
}
void NewtonDemos::OnSelectSolverMode(wxCommandEvent& event)
{
BEGIN_MENU_OPTION();
m_solverModeIndex = dClamp (event.GetId() - ID_SOLVER_MODE, 0, int (sizeof (m_solverModes)/sizeof (m_solverModes[0])));
END_MENU_OPTION();
}
void NewtonDemos::OnSelectSolverQuality(wxCommandEvent& event)
{
BEGIN_MENU_OPTION();
m_solverModeQuality = dClamp(event.GetId() - ID_SOLVER_QUALITY, 0, 1);
END_MENU_OPTION();
}
void dNewtonJointDoubleHingeActuator::SetTargetAngle0(dFloat angle, dFloat minLimit, dFloat maxLimit)
{
dCustomDoubleHingeActuator* const joint = (dCustomDoubleHingeActuator*) m_joint;
joint->SetLimits(dMin(minLimit * DEGREES_TO_RAD, dFloat(0.0f)), dMax(maxLimit * DEGREES_TO_RAD, dFloat(0.0f)));
joint->SetTargetAngle0(dClamp (angle * DEGREES_TO_RAD, joint->GetMinAngularLimit0(), joint->GetMaxAngularLimit0()));
}
void dCustomHingeActuator::SetTargetAngle(dFloat angle)
{
m_angle = dClamp (angle, m_minAngle, m_maxAngle);
}
int dCustomPlayerController::ResolveInterpenetrations(int contactCount, NewtonWorldConvexCastReturnInfo* const contactArray)
{
dVector zero (0.0f);
dVector veloc (0.0f);
dVector savedVeloc (0.0f);
NewtonBodyGetVelocity(m_kinematicBody, &savedVeloc[0]);
NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
dFloat timestep = 0.1f;
dFloat invTimestep = 1.0f / timestep;
dComplementaritySolver::dJacobian jt[D_MAX_ROWS];
dFloat rhs[D_MAX_ROWS];
dFloat low[D_MAX_ROWS];
dFloat high[D_MAX_ROWS];
int normalIndex[D_MAX_ROWS];
NewtonWorld* const world = m_manager->GetWorld();
for (int i = 0; i < 3; i++) {
jt[i].m_linear = zero;
jt[i].m_angular = zero;
jt[i].m_angular[i] = dFloat(1.0f);
rhs[i] = 0.0f;
low[i] = -1.0e12f;
high[i] = 1.0e12f;
normalIndex[i] = 0;
}
dFloat penetration = D_MAX_COLLISION_PENETRATION * 10.0f;
for (int j = 0; (j < 8) && (penetration > D_MAX_COLLISION_PENETRATION) ; j ++) {
dMatrix matrix;
dVector com(0.0f);
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]);
com = matrix.TransformVector(com);
com.m_w = 0.0f;
int rowCount = 3;
for (int i = 0; i < contactCount; i++) {
NewtonWorldConvexCastReturnInfo& contact = contactArray[i];
dVector point(contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
dVector normal(contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f);
jt[rowCount].m_linear = normal;
jt[rowCount].m_angular = (point - com).CrossProduct(normal);
low[rowCount] = 0.0f;
high[rowCount] = 1.0e12f;
normalIndex[rowCount] = 0;
penetration = dClamp(contact.m_penetration - D_MAX_COLLISION_PENETRATION * 0.5f, dFloat(0.0f), dFloat(0.5f));
rhs[rowCount] = penetration * invTimestep;
rowCount++;
}
dVector impulse (CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt));
impulse = impulse.Scale(m_invMass);
NewtonBodySetVelocity(m_kinematicBody, &impulse[0]);
NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);
NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, contactArray, 4, 0);
penetration = 0.0f;
for (int i = 0; i < contactCount; i++) {
penetration = dMax(contactArray[i].m_penetration, penetration);
}
}
NewtonBodySetVelocity(m_kinematicBody, &savedVeloc[0]);
return contactCount;
}
void CustomJoint::SetStiffness(dFloat stiffness)
{
m_stiffness = dClamp (stiffness, dFloat(0.0f), dFloat(1.0f));
}
dBigVector dBezierSpline::CurvePoint (dFloat64 u) const
{
u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
int span = GetSpan(u);
return CurvePoint (u, span);
}