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(); }