void odCable::run(){
int i;
double pi=4.*atan(1.);
double volume, segLen, rho, x, u, w, alpha, uwMag, alphaMap;
odPoint P0, F, V, axial, radial;
dVector3 P;
// The opendynamics solver handles connections by default (unless we wish to do something odd with them).
// dBodyAddRelForceAtRelPos(element[nSegments-1], 50, 0, 50, 0, 0, 0);
// Axial and radial directions in local element co-ordinates.
axial=odPoint(1,0,0);
radial=odPoint(0,0,1);
segLen=length/(double)nSegments;
volume=pi*pow((diameter/2),2)*segLen;
for (i=0;i<nSegments;i++){
// Temporarily make the current element the active body...
odeBody=&element[i];
// Add buoyant force...
dBodyGetRelPointPos(element[i], 0, 0, 0, P);
P0=odPoint(P[0],P[1],P[2]);
rho=density(P0);
V=localVelocity(P0, uid);
// printf("Centre Location = %lf\t%lf\t%lf\t\n", P[0], P[1], P[2]);
F=odPoint(0,0,1)*constants->g*volume*rho;
// printf("Volume = %lf\n", volume);
// printf("Density= %lf\n", density(odPoint(P[0],P[1],P[2])));
// printf("Buoyant Force = %lf\t%lf\t%lf\n", F.x, F.y, F.z);
// dBodyAddForceAtRelPos(element[nSegments-1], F.x, F.y, F.z, 0, 0, 0);
if (i==0){
// Add resistance force...
V.x=1;
V.y=0;
V.z=0;
printf("Flow Speed = %lf\t%lf\t%lf\n", V.x, V.y, V.z);
u=radial.dotProduct_natural(-V);
w=axial.dotProduct_natural(-V);
uwMag=sqrt(u*u+w*w);
printf("u=%lf\tw=%lf\tuwMAG=%lf\n",u,w,uwMag);
alpha=atan2(w,u);
printf("Alpha %lf\n",alpha*180./pi);
// map to 0-90 range for convenience...
alphaMap=fabs(alpha*180./pi);
if (alphaMap>90) alphaMap=90-alphaMap;
printf("AlphaMap %lf\n",alphaMap);
// x=0.5*rho*V.mag()*segLen*diameter*0.5;
// if (*simTime<20.) x*=*simTime/20.;
// F=-V/V.mag()*x; // Multiply by the inverse of the velocity vector (if the body is moving with v- then drag is in the direction of v+
printf("Drag Force = %lf\t%lf\t%lf\n", F.x, F.y, F.z);
// dBodyAddForceAtRelPos(element[nSegments-1], F.x, F.y, F.z, 0, 0, 0);
}
}
}
void DustParticleSystem::initParticles(int numParticles, void* particleData, hkReal timestep, const void* emitParams)
{
HK_ASSERT(0x6f68d095, emitParams);
const EmitParams* params = static_cast<const EmitParams*>(emitParams);
hkMatrix3 orthonormalBasis;
hkVector4Util::buildOrthonormal(params->m_emitDirection, orthonormalBasis);
const hkReal minRadiusFractions[4] = { 2.f, 0.4f, 0.4f, 0.2f };
const hkReal maxRadiusFractions[4] = { 3.f, 2.f, 2.f, 1.f };
const hkReal outOfPlaneVarianceScales[4] = { 0.01f, 0.1f, 0.1f, 0.05f };
for(int i = 0; i < numParticles; ++i)
{
Particle& p = *reinterpret_cast<Particle*>(static_cast<char*>(particleData) + i*m_particleStride);
p.m_position = params->m_emitPosition;
p.m_position(3) = m_pseudoRandomGenerator.getRandReal01() * 2.f * HK_REAL_PI;
// Choose a sprite for the particle.
int spriteIndex;
hkReal rnd = m_pseudoRandomGenerator.getRandReal01();
if(rnd < 0.25f)
{
spriteIndex = 1;
}
else if(rnd < 0.5f)
{
spriteIndex = 2;
}
else if(rnd < 0.7f)
{
spriteIndex = 3;
}
else
{
spriteIndex = 0;
}
hkReal radius = m_pseudoRandomGenerator.getRandRange(minRadiusFractions[spriteIndex], maxRadiusFractions[spriteIndex]) * params->m_speedScale;
hkReal theta = m_pseudoRandomGenerator.getRandReal01() * 2.f * HK_REAL_PI;
hkReal ringX = radius * hkMath::cos(theta);
hkReal ringY = radius * hkMath::sin(theta);
hkReal outOfPlaneVariance = outOfPlaneVarianceScales[spriteIndex] * params->m_speedScale;
hkVector4 localVelocity(m_pseudoRandomGenerator.getRandRange(-outOfPlaneVariance, outOfPlaneVariance), ringY, ringX);
orthonormalBasis.multiplyVector(localVelocity, p.m_velocity);
// Set the spin direction based on whether the particle is moving towards the left or right.
hkReal spinRate = m_pseudoRandomGenerator.getRandRange(params->m_minSpinRate, params->m_maxSpinRate);
if(params->m_planeNormal.dot3(p.m_velocity) > 0.f)
spinRate = -spinRate;
p.m_velocity(3) = spinRate;
p.m_lifeTime = m_pseudoRandomGenerator.getRandRange(params->m_minLifeTime, params->m_maxLifeTime);
p.m_age = m_pseudoRandomGenerator.getRandReal01() * timestep;
p.m_size = m_pseudoRandomGenerator.getRandRange(params->m_minSize, params->m_maxSize);
p.m_spriteIndex = static_cast<hkReal>(spriteIndex);
// Particle positions are initially integrated by a random amount to make it seem as
// though they are emitted continuously over time instead of at discrete time steps.
p.m_position.addMul4(p.m_age, p.m_velocity);
}
}
void MayaCamera::Update(float timestep)
{
(void)timestep;
// Track mouse motion
int2 mousePos;
GetCursorPos((POINT *)&mousePos);
int2 mouseMove = mousePos - m_mousePosPrev;
m_mousePosPrev = mousePos;
// Handle mouse rotation
if (m_mbuttonCur == MBUTTON_Left)
{
m_yaw -= m_rotateSpeed * mouseMove.x;
m_yaw = modPositive(m_yaw, 2.0f*pi);
m_pitch -= m_rotateSpeed * mouseMove.y;
m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
}
// Retrieve controller state
XINPUT_STATE controllerState = {};
float2 controllerLeftStick(0.0f), controllerRightStick(0.0f);
float controllerLeftTrigger = 0.0f, controllerRightTrigger = 0.0f;
if (m_controllerPresent)
{
// Look out for disconnection
if (XInputGetState(0, &controllerState) == ERROR_SUCCESS)
{
// Decode axes and apply dead zones
controllerLeftTrigger = float(max(0, controllerState.Gamepad.bLeftTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
controllerRightTrigger = float(max(0, controllerState.Gamepad.bRightTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
controllerLeftStick = float2(controllerState.Gamepad.sThumbLX, controllerState.Gamepad.sThumbLY);
float lengthLeft = length(controllerLeftStick);
if (lengthLeft > XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE)
controllerLeftStick = (controllerLeftStick / lengthLeft) * (lengthLeft - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
else
controllerLeftStick = float2(0.0f);
controllerRightStick = float2(controllerState.Gamepad.sThumbRX, controllerState.Gamepad.sThumbRY);
float lengthRight = length(controllerRightStick);
if (lengthRight > XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE)
controllerRightStick = (controllerRightStick / lengthRight) * (lengthRight - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE);
else
controllerRightStick = float2(0.0f);
}
else
{
m_controllerPresent = false;
}
}
// Handle controller rotation
if (m_controllerPresent)
{
m_yaw -= m_controllerRotateSpeed * controllerRightStick.x * abs(controllerRightStick.x) * timestep;
m_yaw = modPositive(m_yaw, 2.0f*pi);
m_pitch += m_controllerRotateSpeed * controllerRightStick.y * abs(controllerRightStick.y) * timestep;
m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
}
UpdateOrientation();
// Handle zoom
if (m_mbuttonCur == MBUTTON_Right)
{
m_radius *= expf(mouseMove.y * m_zoomSpeed);
}
m_radius *= expf(-m_wheelDelta * m_zoomWheelSpeed);
m_wheelDelta = 0;
// Handle controller zoom
if (m_controllerPresent && !(controllerState.Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER))
{
m_radius *= expf(-controllerLeftStick.y * abs(controllerLeftStick.y) * m_controllerZoomSpeed * timestep);
}
// Handle motion of target point
if (m_mbuttonCur == MBUTTON_Middle)
{
m_posTarget -= m_rotateSpeed * mouseMove.x * m_radius * m_viewToWorld[0].xyz;
m_posTarget += m_rotateSpeed * mouseMove.y * m_radius * m_viewToWorld[1].xyz;
}
// Handle controller motion of target point
if (m_controllerPresent && (controllerState.Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER))
{
float3 localVelocity(0.0f);
localVelocity.x = controllerLeftStick.x * abs(controllerLeftStick.x);
localVelocity.y = square(controllerRightTrigger) - square(controllerLeftTrigger);
localVelocity.z = -controllerLeftStick.y * abs(controllerLeftStick.y);
m_posTarget += xfmVector(localVelocity, m_viewToWorld) * (m_radius * m_controllerMoveSpeed * timestep);
}
// Calculate remaining matrices
m_pos = m_posTarget + m_radius * m_viewToWorld[2].xyz;
setTranslation(&m_viewToWorld, m_pos);
UpdateWorldToClip();
}
void CustomVehicleController::DrawSchematic (dFloat scale) const
{
dVector array [32];
dMatrix projectionMatrix (dGetIdentityMatrix());
projectionMatrix[0][0] = scale;
projectionMatrix[1][1] = 0.0f;
projectionMatrix[2][1] = scale;
projectionMatrix[2][2] = 0.0f;
CustomVehicleControllerManager* const manager = (CustomVehicleControllerManager*)GetManager();
const dMatrix& chassisMatrix = m_chassisState.GetMatrix();
const dMatrix& chassisFrameMatrix = m_chassisState.GetLocalMatrix();
dMatrix worldToComMatrix ((chassisFrameMatrix * chassisMatrix).Inverse() * projectionMatrix);
{
// draw vehicle chassis
dVector p0 (1.0e10f, 1.0e10f, 1.0e10f, 0.0f);
dVector p1 (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f);
for (dList<CustomVehicleControllerBodyStateTire>::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
dMatrix matrix (tire->CalculateSteeringMatrix() * m_chassisState.GetMatrix());
dVector p (worldToComMatrix.TransformVector(matrix.m_posit));
p0 = dVector (dMin (p.m_x, p0.m_x), dMin (p.m_y, p0.m_y), dMin (p.m_z, p0.m_z), 1.0f);
p1 = dVector (dMax (p.m_x, p1.m_x), dMax (p.m_y, p1.m_y), dMax (p.m_z, p1.m_z), 1.0f);
}
array[0] = dVector (p0.m_x, p0.m_y, p0.m_z, 1.0f);
array[1] = dVector (p1.m_x, p0.m_y, p0.m_z, 1.0f);
array[2] = dVector (p1.m_x, p1.m_y, p0.m_z, 1.0f);
array[3] = dVector (p0.m_x, p1.m_y, p0.m_z, 1.0f);
manager->DrawSchematicCallback(this, "chassis", 0, 4, array);
}
{
// draw vehicle tires
for (dList<CustomVehicleControllerBodyStateTire>::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
dFloat width = tire->m_width * 0.5f;
dFloat radio = tire->m_radio;
dMatrix matrix (tire->CalculateSteeringMatrix() * m_chassisState.GetMatrix());
array[0] = worldToComMatrix.TransformVector(matrix.TransformVector(dVector ( width, 0.0f, radio, 0.0f)));
array[1] = worldToComMatrix.TransformVector(matrix.TransformVector(dVector ( width, 0.0f, -radio, 0.0f)));
array[2] = worldToComMatrix.TransformVector(matrix.TransformVector(dVector (-width, 0.0f, -radio, 0.0f)));
array[3] = worldToComMatrix.TransformVector(matrix.TransformVector(dVector (-width, 0.0f, radio, 0.0f)));
manager->DrawSchematicCallback(this, "tire", 0, 4, array);
}
}
{
// draw vehicle velocity
//dVector veloc1;
//NewtonBodyGetVelocity(GetBody(), &veloc[0]);
dVector veloc (m_chassisState.GetVelocity());
//dVector xxx (veloc1 - veloc);
//dAssert (dAbs(xxx % xxx) < 1.0e-3f);
dVector localVelocity (chassisFrameMatrix.UnrotateVector (chassisMatrix.UnrotateVector (veloc)));
localVelocity.m_y = 0.0f;
localVelocity = projectionMatrix.RotateVector(localVelocity);
array[0] = dVector (0.0f, 0.0f, 0.0f, 0.0f);
array[1] = localVelocity.Scale (0.25f);
manager->DrawSchematicCallback(this, "velocity", 0, 2, array);
}
{
dFloat scale (2.0f / (m_chassisState.GetMass() * m_chassisState.m_gravityMag));
// draw vehicle forces
for (dList<CustomVehicleControllerBodyStateTire>::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
//dVector p0 (tire->GetCenterOfMass());
dMatrix matrix (tire->CalculateSteeringMatrix() * m_chassisState.GetMatrix());
//dTrace (("(%f %f %f) (%f %f %f)\n", p0.m_x, p0.m_y, p0.m_z, matrix.m_posit.m_x, matrix.m_posit.m_y, matrix.m_posit.m_z ));
dVector origin (worldToComMatrix.TransformVector(matrix.m_posit));
dVector lateralForce (chassisFrameMatrix.UnrotateVector(chassisMatrix.UnrotateVector(tire->GetLateralForce())));
lateralForce = lateralForce.Scale (-scale);
lateralForce = projectionMatrix.RotateVector (lateralForce);
//dTrace (("(%f %f %f)\n", lateralForce.m_x, lateralForce.m_y, lateralForce.m_z ));
array[0] = origin;
array[1] = origin + lateralForce;
manager->DrawSchematicCallback(this, "lateralForce", 0, 2, array);
dVector longitudinalForce (chassisFrameMatrix.UnrotateVector(chassisMatrix.UnrotateVector(tire->GetLongitudinalForce())));
longitudinalForce = longitudinalForce.Scale (-scale);
longitudinalForce = projectionMatrix.RotateVector (longitudinalForce);
//dTrace (("(%f %f %f)\n", lateralForce.m_x, lateralForce.m_y, lateralForce.m_z ));
array[0] = origin;
array[1] = origin + longitudinalForce;
manager->DrawSchematicCallback(this, "longitudinalForce", 0, 2, array);
// dVector p2 (p0 - tire->GetLateralForce().Scale (scale));
/*
// offset the origin of of tire force so that they are visible
const dMatrix& tireMatrix = tire.GetLocalMatrix ();
p0 += chassis.GetMatrix()[2].Scale ((tireMatrix.m_posit.m_z > 0.0f ? 1.0f : -1.0f) * 0.25f);
// draw the tire load
dVector p1 (p0 + tire.GetTireLoad().Scale (scale));
glColor3f (0.0f, 0.0f, 1.0f);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
// show tire lateral force
dVector p2 (p0 - tire.GetLateralForce().Scale (scale));
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p2.m_x, p2.m_y, p2.m_z);
// show tire longitudinal force
dVector p3 (p0 - tire.GetLongitudinalForce().Scale (scale));
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p3.m_x, p3.m_y, p3.m_z);
*/
}
}
}
void FPSCamera::Update(float timestep)
{
// Track mouse motion
int2 mousePos;
GetCursorPos((POINT *)&mousePos);
int2 mouseMove = mousePos - m_mousePosPrev;
m_mousePosPrev = mousePos;
// Handle mouse rotation
if (m_mbuttonActivate == MBUTTON_None ||
m_mbuttonCur == m_mbuttonActivate)
{
m_yaw -= m_rotateSpeed * mouseMove.x;
m_yaw = modPositive(m_yaw, 2.0f*pi);
m_pitch -= m_rotateSpeed * mouseMove.y;
m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
}
// Retrieve controller state
XINPUT_STATE controllerState = {};
float2 controllerLeftStick(0.0f), controllerRightStick(0.0f);
float controllerLeftTrigger = 0.0f, controllerRightTrigger = 0.0f;
if (m_controllerPresent)
{
// Look out for disconnection
if (XInputGetState(0, &controllerState) == ERROR_SUCCESS)
{
// Decode axes and apply dead zones
controllerLeftTrigger = float(max(0, controllerState.Gamepad.bLeftTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
controllerRightTrigger = float(max(0, controllerState.Gamepad.bRightTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
controllerLeftStick = float2(controllerState.Gamepad.sThumbLX, controllerState.Gamepad.sThumbLY);
float lengthLeft = length(controllerLeftStick);
if (lengthLeft > XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE)
controllerLeftStick = (controllerLeftStick / lengthLeft) * (lengthLeft - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
else
controllerLeftStick = float2(0.0f);
controllerRightStick = float2(controllerState.Gamepad.sThumbRX, controllerState.Gamepad.sThumbRY);
float lengthRight = length(controllerRightStick);
if (lengthRight > XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE)
controllerRightStick = (controllerRightStick / lengthRight) * (lengthRight - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE);
else
controllerRightStick = float2(0.0f);
}
else
{
m_controllerPresent = false;
}
}
// Handle controller rotation
if (m_controllerPresent)
{
m_yaw -= m_controllerRotateSpeed * controllerRightStick.x * abs(controllerRightStick.x) * timestep;
m_yaw = modPositive(m_yaw, 2.0f*pi);
m_pitch += m_controllerRotateSpeed * controllerRightStick.y * abs(controllerRightStick.y) * timestep;
m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
}
UpdateOrientation();
// Handle translation
// !!!UNDONE: acceleration based on how long you've been holding the button,
// to make fine motions easier?
float moveStep = timestep * m_moveSpeed;
// !!!UNDONE: move keyboard tracking into an input system that respects focus, etc.
if (GetAsyncKeyState(VK_SHIFT) || (controllerState.Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER))
moveStep *= 3.0f;
if (GetAsyncKeyState('W'))
m_pos -= m_viewToWorld[2].xyz * moveStep;
if (GetAsyncKeyState('S'))
m_pos += m_viewToWorld[2].xyz * moveStep;
if (GetAsyncKeyState('A'))
m_pos -= m_viewToWorld[0].xyz * moveStep;
if (GetAsyncKeyState('D'))
m_pos += m_viewToWorld[0].xyz * moveStep;
if (GetAsyncKeyState('E'))
m_pos += m_viewToWorld[1].xyz * moveStep;
if (GetAsyncKeyState('C'))
m_pos -= m_viewToWorld[1].xyz * moveStep;
if (m_controllerPresent)
{
float3 localVelocity(0.0f);
localVelocity.x = controllerLeftStick.x * abs(controllerLeftStick.x);
localVelocity.y = square(controllerRightTrigger) - square(controllerLeftTrigger);
localVelocity.z = -controllerLeftStick.y * abs(controllerLeftStick.y);
m_pos += xfmVector(localVelocity, m_viewToWorld) * (moveStep * m_controllerMoveSpeed);
}
// Calculate remaining matrices
setTranslation(&m_viewToWorld, m_pos);
UpdateWorldToClip();
}
