void CustomSlider::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);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit + matrix1.m_front.Scale((p0 - matrix1.m_posit) % matrix1.m_front));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
// three rows to restrict rotation around around the parent coordinate system
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front), &matrix1.m_front[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// calculate position and speed
dVector veloc0(0.0f);
dVector veloc1(0.0f);
dAssert (m_body0);
NewtonBodyGetPointVelocity(m_body0, &matrix0.m_posit[0], &veloc0[0]);
if (m_body1) {
NewtonBodyGetPointVelocity(m_body1, &matrix1.m_posit[0], &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
m_lastRowWasUsed = false;
SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
const dFloat angleError = GetMaxAngleError();
dFloat angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up);
NewtonUserJointAddAngularRow(m_joint, angle0, &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
if (dAbs(angle0) > angleError) {
const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle0 / (timestep * timestep);
NewtonUserJointSetRowAcceleration(m_joint, alpha);
}
dFloat angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right);
NewtonUserJointAddAngularRow(m_joint, angle1, &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
if (dAbs(angle1) > angleError) {
const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle1 / (timestep * timestep);
NewtonUserJointSetRowAcceleration(m_joint, alpha);
}
// the joint angle can be determined by getting the angle between any two non parallel vectors
m_curJointAngle.Update(-CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front));
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
if (m_options.m_option2) {
if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
} else {
dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
}
} else if (m_options.m_option3) {
dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
} else if (m_angularFriction != 0.0f) {
NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
}
}
void dCustomHinge::SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dFloat sinAngle;
dFloat cosAngle;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// two rows to restrict rotation around around the parent coordinate system
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
// the joint angle can be determined by getting the angle between any two non parallel vectors
CalculateAngle (matrix1.m_up, matrix0.m_up, matrix1.m_front, sinAngle, cosAngle);
m_curJointAngle.Update(cosAngle, sinAngle);
// save the current joint Omega
dVector omega0(0.0f);
dVector omega1(0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
m_jointOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);
m_lastRowWasUsed = false;
if (m_setAsSpringDamper) {
ApplySpringDamper (timestep, matrix0, matrix1);
} else {
SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
}
void CParticleManager::SetProperties(int cont, int i)
{
CParticleEmitter* l_pE = new CParticleEmitter();
l_pE->SetMinLife(m_vInfo[cont].fLifeMin);
l_pE->SetMaxLife(m_vInfo[cont].fLifeMax);
l_pE->SetTexture(m_vInfo[cont].pTexture);
l_pE->SetMaxEmitRate(m_vInfo[cont].fEmitRate2);
l_pE->SetMinEmitRate(m_vInfo[cont].fEmitRate1);
l_pE->SetGravity(m_vInfo[cont].bGravity);
l_pE->SetName(m_vInfo[cont].sName);
//calcular posiciones del emisor
l_pE->SetPos1(CalculatePos1(cont, i));
l_pE->SetPos2(CalculatePos2(cont, i));
//calcular color, dirección, tamaño, velocidad y ángulo de rotación
CalculateColor(cont);
CalculateDir(cont);
CalculateSize(cont);
CalculateVelocity(cont);
CalculateAngle(cont);
//fijar color, dirección, tamaño, velocidad y ángulo de rotación
l_pE->SetColorTime(m_vTimeColor);
l_pE->SetColorV(m_vColor);
l_pE->SetDirTime(m_vTimeDir);
l_pE->SetDirV(m_vDir);
l_pE->SetSizeTime(m_vTimeSize);
l_pE->SetSizeV(m_vSize);
l_pE->SetVelocityTime(m_vTimeVel);
l_pE->SetVelocityV(m_vVelocity);
l_pE->SetAngleTime(m_vTimeAngle);
l_pE->SetAngleV(m_vAngle);
//borrar memoria
m_vInfo[cont].vColor.clear();
m_vInfo[cont].vDir.clear();
m_vInfo[cont].vSize.clear();
m_vInfo[cont].vVelocity.clear();
m_vInfo[cont].vAngle.clear();
m_vParticleEmitter.push_back(l_pE);
}
// This is converting DAT Winpilot
int ParseDAT(TCHAR *String,WAYPOINT *Temp)
{
TCHAR ctemp[(COMMENT_SIZE*2)+1]; // 101102 BUGFIX, we let +1 for safety
TCHAR *Number;
TCHAR *pWClast = NULL;
TCHAR *pToken;
TCHAR TempString[READLINE_LENGTH];
_tcscpy(TempString, String);
// 20060513:sgi added wor on a copy of the string, do not modify the
// source string, needed on error messages
Temp->Visible = true; // default all waypoints visible at start
Temp->FarVisible = true;
Temp->Format = LKW_DAT;
Temp->FileNum = globalFileNum;
// ExtractParameter(TempString,ctemp,0);
if ((pToken = _tcstok_r(TempString, TEXT(","), &pWClast)) == NULL)
return FALSE;
Temp->Number = _tcstol(pToken, &Number, 10);
//ExtractParameter(TempString,ctemp,1); //Latitude
if ((pToken = _tcstok_r(NULL, TEXT(","), &pWClast)) == NULL)
return FALSE;
Temp->Latitude = CalculateAngle(pToken);
if((Temp->Latitude > 90) || (Temp->Latitude < -90))
{
return FALSE;
}
//ExtractParameter(TempString,ctemp,2); //Longitude
if ((pToken = _tcstok_r(NULL, TEXT(","), &pWClast)) == NULL)
return FALSE;
Temp->Longitude = CalculateAngle(pToken);
if((Temp->Longitude > 180) || (Temp->Longitude < -180))
{
return FALSE;
}
//ExtractParameter(TempString,ctemp,3); //Altitude
if ((pToken = _tcstok_r(NULL, TEXT(","), &pWClast)) == NULL)
return FALSE;
Temp->Altitude = ReadAltitude(pToken);
if (Temp->Altitude == -9999){
return FALSE;
}
//ExtractParameter(TempString,ctemp,4); //Flags
if ((pToken = _tcstok_r(NULL, TEXT(","), &pWClast)) == NULL)
return FALSE;
Temp->Flags = CheckFlags(pToken);
//ExtractParameter(TempString,ctemp,5); // Name
if ((pToken = _tcstok_r(NULL, TEXT(",\n\r"), &pWClast)) == NULL)
return FALSE;
// guard against overrun
if (_tcslen(pToken)>NAME_SIZE) {
pToken[NAME_SIZE-1]= _T('\0');
}
_tcscpy(Temp->Name, pToken);
int i;
for (i=_tcslen(Temp->Name)-1; i>1; i--) {
if (Temp->Name[i]==' ') {
Temp->Name[i]=0;
} else {
break;
}
}
//ExtractParameter(TempString,ctemp,6); // Comment
// DAT Comment
if ((pToken = _tcstok_r(NULL, TEXT("\n\r"), &pWClast)) != NULL){
LK_tcsncpy(ctemp, pToken, COMMENT_SIZE); //@ 101102 BUGFIX bad. ctemp was not sized correctly!
if (_tcslen(ctemp) >0 ) {
if (Temp->Comment) {
free(Temp->Comment);
}
Temp->Comment = (TCHAR*)malloc((_tcslen(ctemp)+1)*sizeof(TCHAR));
if (Temp->Comment) _tcscpy(Temp->Comment, ctemp);
}
} else {
Temp->Comment = NULL; // useless
}
if(Temp->Altitude <= 0) {
WaypointAltitudeFromTerrain(Temp);
}
if (Temp->Details) {
free(Temp->Details);
}
return TRUE;
}
dFloat CustomJoint::CalculateAngle (const dVector& dir, const dVector& cosDir, const dVector& sinDir) const
{
dFloat sinAngle;
dFloat cosAngle;
return CalculateAngle (dir, cosDir, sinDir, sinAngle, cosAngle);
}
long CalculateAngle(const POINT& pt1, const POINT& pt2)
{
return CalculateAngle(pt1.x, pt1.y, pt2.x, pt2.y);
}
void CustomLimitBallAndSocket::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);
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
matrix1 = m_rotationOffset * matrix1;
// handle special case of the joint being a hinge
if (m_coneAngleCos > 0.9999f) {
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// the joint angle can be determined by getting the angle between any two non parallel vectors
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
} else {
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngle = coneDir0 % coneDir1;
if (cosAngle <= m_coneAngleCos) {
dVector lateralDir(coneDir0 * coneDir1);
dFloat mag2 = lateralDir % lateralDir;
dAssert(mag2 > 1.0e-4f);
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
dQuaternion rot(m_coneAngleHalfCos, lateralDir.m_x * m_coneAngleHalfSin, lateralDir.m_y * m_coneAngleHalfSin, lateralDir.m_z * m_coneAngleHalfSin);
dVector frontDir(rot.UnrotateVector(coneDir1));
dVector upDir(lateralDir * frontDir);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(coneDir0, frontDir, lateralDir), &lateralDir[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
}
//handle twist angle
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
}
}
void CustomUniversal::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);
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
// construct an orthogonal coordinate system with these two vectors
dMatrix matrix1_1;
matrix1_1.m_up = matrix1.m_up;
matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
matrix1_1.m_right = matrix1_1.m_right.Scale (1.0f / dSqrt (matrix1_1.m_right % matrix1_1.m_right));
matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
NewtonUserJointAddAngularRow (m_joint, CalculateAngle (matrix0.m_front, matrix1_1.m_front, matrix1_1.m_right), &matrix1_1.m_right[0]);
dFloat sinAngle_0;
dFloat cosAngle_0;
CalculateAngle (matrix1_1.m_up, matrix0.m_up, matrix1_1.m_front, sinAngle_0, cosAngle_0);
dFloat angle0 = -m_curJointAngle_0.Update (cosAngle_0, sinAngle_0);
dFloat sinAngle_1;
dFloat cosAngle_1;
CalculateAngle(matrix1.m_front, matrix1_1.m_front, matrix1_1.m_up, sinAngle_1, cosAngle_1);
dFloat angle1 = -m_curJointAngle_1.Update (cosAngle_1, sinAngle_1);
dVector omega0 (0.0f, 0.0f, 0.0f, 0.0f);
dVector omega1 (0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodyGetOmega(m_body0, &omega0[0]);
if (m_body1) {
NewtonBodyGetOmega(m_body1, &omega1[0]);
}
// calculate the desired acceleration
dVector relOmega (omega0 - omega1);
m_jointOmega_0 = relOmega % matrix0.m_front;
m_jointOmega_1 = relOmega % matrix1.m_up;
// check is the joint limit are enable
if (m_limit_0_On) {
if (angle0 < m_minAngle_0) {
dFloat relAngle = angle0 - m_minAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle0 > m_maxAngle_0) {
dFloat relAngle = angle0 - m_maxAngle_0;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix0.m_front[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
// check is the joint limit motor is enable
} else if (m_angularMotor_0_On) {
// calculate the desired acceleration
// dFloat relOmega = (omega0 - omega1) % matrix0.m_front;
dFloat relAccel = m_angularAccel_0 - m_angularDamp_0 * m_jointOmega_0;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
// if limit are enable ...
if (m_limit_1_On) {
if (angle1 < m_minAngle_1) {
dFloat relAngle = angle1 - m_minAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (angle1 > m_maxAngle_1) {
dFloat relAngle = angle1 - m_maxAngle_1;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow (m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
} else if (m_angularMotor_1_On) {
// calculate the desired acceleration
dFloat relAccel = m_angularAccel_1 - m_angularDamp_1 * m_jointOmega_1;
// add and angular constraint row to that will set the relative acceleration to zero
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix1.m_up[0]);
// override the joint acceleration.
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
}
void CustomSlidingContact::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dFloat sinAngle;
dFloat cosAngle;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit + matrix1.m_front.Scale((p0 - matrix1.m_posit) % matrix1.m_front));
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
// construct an orthogonal coordinate system with these two vectors
dMatrix matrix1_1;
matrix1_1.m_up = matrix1.m_up;
matrix1_1.m_right = matrix0.m_front * matrix1.m_up;
matrix1_1.m_right = matrix1_1.m_right.Scale(1.0f / dSqrt(matrix1_1.m_right % matrix1_1.m_right));
matrix1_1.m_front = matrix1_1.m_up * matrix1_1.m_right;
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_up, matrix1_1.m_up, matrix1_1.m_front), &matrix1_1.m_front[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_up, matrix1_1.m_up, matrix1_1.m_right), &matrix1_1.m_right[0]);
// the joint angle can be determined by getting the angle between any two non parallel vectors
CalculateAngle(matrix1_1.m_front, matrix1.m_front, matrix1.m_up, sinAngle, cosAngle);
m_curJointAngle.Update(cosAngle, sinAngle);
dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f);
dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);
dAssert(m_body0);
NewtonBodyGetPointVelocity(m_body0, &matrix0.m_posit[0], &veloc0[0]);
if (m_body1) {
NewtonBodyGetPointVelocity(m_body1, &matrix1.m_posit[0], &veloc1[0]);
}
m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
m_speed = (veloc0 - veloc1) % matrix1.m_front;
// if limit are enable ...
if (m_limitsLinearOn) {
if (m_posit < m_minLinearDist) {
// get a point along the up vector and set a constraint
dVector p (matrix1.m_posit + matrix1.m_front.Scale(m_minLinearDist));
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &p[0], &matrix1.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
} else if (m_posit > m_maxLinearDist) {
dVector p(matrix1.m_posit + matrix1.m_front.Scale(m_maxLinearDist));
NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &p[0], &matrix1.m_front[0]);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
if (m_limitsAngularOn) {
dFloat angle1 = m_curJointAngle.GetAngle();
if (angle1 < m_minAngularDist) {
dFloat relAngle = angle1 - m_minAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffeners here
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
else if (angle1 > m_maxAngularDist) {
dFloat relAngle = angle1 - m_maxAngularDist;
// tell joint error will minimize the exceeded angle error
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix1.m_up[0]);
// need high stiffness here
NewtonUserJointSetRowStiffness(m_joint, 1.0f);
// allow the joint to move back freely
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
}
}
}
/* ================= main ====================
desc: 程序入口函数
pre: 无
Post: 无
*/
void main(void) {
// Local Declarations
byte laser_history_array[LASER_HISTORY_MAX]; //激光管历史状态数组
Bool temp_usualRoad_flag = TRUE; //当前判断是否正常路段标志
Bool last_usualRoad_flag = TRUE; //上次判断是否正常路段标志
Bool stopCar_flag = FALSE; //停车标志
Bool breakCar_flag = FALSE; //刹车标志
byte startlineFlag_count = 0; //经过起始线的次数
byte laser_hitNum = 1; //照到黑线的激光管个数
byte inside_count = INSIDE_COUNT_MAX; //激光管连续在黑线范围内的次数
byte outside_count = 0; //激光管连续在黑线外的次数
byte last_error = 0; //直道加速匀速控制的上次误差
LASER_STATUS last_laserStatus = MID7; //上次激光管状态
LASER_STATUS temp_laserStatus = MID7; //当前激光管状态
int last_turnAngle = PWM1_MID; //上次舵机调整的角度
int temp_turnAngle = PWM1_MID; //当前舵机需要调整的角度
int last_speed = PWM3_FAST0; //上次速度
int temp_speed = PWM3_FAST0; //当前速度
int i;
int testcount=0; //发送激光管信息计数值定义
for(i=0;i<LASER_HISTORY_MAX;i++) {
laser_history_array[i] = MID7;
}
// Statements
DisableInterrupts;
MCUInit();
SmartcarInit();
EnableInterrupts;
for(;;) {
if(PITINTE_PINTE0 == 0) { //若PIT0采集中断为关,即道路信息采集完成
laser_hitNum = 15 - CalculateLaserHitNum(g_temp_laser_array);
temp_usualRoad_flag = IsUsualRoad (laser_hitNum); //判断是否为正常道路
if (temp_usualRoad_flag) {
temp_laserStatus = GetLaserStatus(last_laserStatus,g_temp_laser_array,laser_hitNum,&inside_count,&breakCar_flag,laser_history_array,&outside_count); //得到当前激光管状态
temp_turnAngle = CalculateAngle(temp_laserStatus); //得到舵机需要调整的转角
temp_speed = CalculateSpeed (temp_turnAngle,stopCar_flag,inside_count,outside_count); //得到需要输出的速度
}
else {
if((last_usualRoad_flag == TRUE)&&(laser_hitNum>=8&&laser_hitNum<=11)) { //一定执行
startlineFlag_count = CountStartlineFlag(startlineFlag_count,g_temp_laser_array); //计算小车经过起始线的次数
if(startlineFlag_count == 2)
stopCar_flag = TRUE; //若是第二次经过起始线，停车标志置位，即停车
StopCar(stopCar_flag);
}
} /**/
testcount++;
if(testcount%50==0){
testcount=1;
SendSmartcarInfo(g_temp_laser_array,temp_laserStatus,last_laserStatus,g_temp_pulse);//发送激光管信息
} /* */
PITINTE_PINTE0 = 1; //开PIT0采集中断
}
DerectionCtrl(temp_turnAngle); //调整舵机
if(breakCar_flag == TRUE) { //若直道入弯，反转减速刹车
BreakCar(g_temp_pulse, &breakCar_flag);
}
else
SpeedCtrl(temp_speed,g_temp_pulse,&last_error); //调整正转速度
last_speed = temp_speed; //保存当前速度
last_laserStatus = temp_laserStatus; //保存当前激光管状态
last_turnAngle = temp_turnAngle; //保存当前舵机转角
last_usualRoad_flag = temp_usualRoad_flag; //保存当前是否正常道路的标志
for(i=LASER_HISTORY_MAX-1;i>0;i--){ //保存激光管历史状态
laser_history_array[i] = laser_history_array[i-1];
}
laser_history_array[0] = temp_laserStatus;
}
} //main