I'm currently building a simplified Reaction Control System for a Satellite game, and need a way to use the system to align the satellite to a given unit direction in world-space coordinates. Because this is a game simulation, I am faking the system and just applying a torque force around the objects epicenter.
This is difficult because in my case, the Torque cannot be varied in strength, it is either on or off. It's either full force or no force. Calculating the direction that the torque needs to be applied in is relatively easy, but I'm having trouble getting it to align perfectly without spinning out of control and getting stuck in a logical loop. it needs to apply the opposing force at precisely the right 'time' to land on the target orientation with zero angular velocity.
What I've determined so far is that I need to calculate the 'time' it will take to reach zero velocity based on my current angular velocity and the angle between the two vectors. If that exceeds the time until I reach angle zero, then it needs to apply the opposing torque. In theory this will also prevent it from 'bouncing' around the axis too much. I almost have it working, but in some cases it seems to get stuck applying force in one direction, so I'm hoping somebody can check the logic. My simulation does NOT take mass into account at the moment, so you can ignore the Inertia Tensor (unless it makes the calculation easier!)
For one axis, I'm currently doing it this way, but I figure someone will have a far more elegant solution that can actually compute both Yaw and Pitch axes at once (Roll is invalid).
Omega = Angular Velocity in Local-Space (Degrees Per Second)
Force = Strength of the Thrusters
// Calculate Time Variables
float Angle = AcosD(DotProduct(ForwardVector, DirectionVector));
float Time1 = Abs(Angle / Omega.Z); // Time taken to reach angle 0 at current velocity
float Time2 = Abs(DeltaTime * (Omega.Z / Force); // Time it will take to reach Zero velocity based on force strength.
// Calculate Direction we need to apply the force to rotate toward the target direction. Note that if we are at perfect opposites, this will be zero!
float AngleSign = Sign(DotProduct(RightVector, DirectionVector));
float Torque.Z = 0;
if (Time1 < Time2)
{
Torque.Z = AngleSign * Force;
}
else
{
Torque.Z = AngleSign * Force * -1.0f
}
// Torque is applied to object as a change in acceleration (no mass) and modified by DeltaSeconds for frame-rate independent force.
This is far from elegant and there are definitely some sign issues. Do you folks know a better way to achieve this?
EDIT:
If anybody understands Unreal Engine's Blueprint system, this is how I'm currently prototyping it before I move it to C++
Beginning from the "Calculate Direction" line, you could instead directly compute the correction torque vector in 3D, then modify its sign if you know that the previous correction is about to overshoot:
// Calculate Direction we need to apply the force to rotate toward the target direction
Torque = CrossProduct(DirectionVector, ForwardVector)
Torque = Normalize(Torque) * Force
if (Time2 < Time1)
{
Torque = -Torque
}
But you should handle the problematic cases:
// Calculate Direction we need to apply the force to rotate toward the target direction
Torque = CrossProduct(DirectionVector, ForwardVector)
if (Angle < 0.1 degrees)
{
// Avoid divide by zero in Normalize
Torque = {0, 0, 0}
}
else
{
// Handle case of exactly opposite direction (where CrossProduct is zero)
if (Angle > 179.9 degrees)
{
Torque = {0, 0, 1}
}
Torque = Normalize(Torque) * Force
if (Time2 < Time1)
{
Torque = -Torque
}
}
Okay well what i take from the pseudocode above is that you want to start braking when the time needed to break exceeds the time left till angle 0 is reached. Have you tried to slowly start breaking (in short steps because of the constant torque) BEFORE the time to break exceeds the time till angle 0?
When you do so and your satellite is near angle 0 and the velocity very low, you can just set velocity and angle to 0 so it doesn't wobble around anymore.
Did you ever figure this out? I'm working on a similar problem in UE4. I also have a constant force. I'm rotating to a new forward vector. I've realized time can't be predicted. Take for example you're rotating on Z axis at 100 degrees/second and a reverse force in exactly .015 seconds will nail your desired rotation and velocity but the next frame takes .016 seconds to render and you've just overshot it since you aren't changing your force. I think the solution is something like cheating by manually setting the forward vector once velocity is zeroed out.
I'm having difficulty getting the Chipmunk physics engine to do what I want. The only solution that appears to work requires some heavy vector math. Before diving into that rabbit hole for the other components of my game, I was hoping someone could fill me in on a better way to go about this. The desired gameplay is as follows:
A character moves around a finite space in a top-down view
Movement is always a constant velocity in whatever direction the character faces
The player taps on the screen, which causes the character to 'turn' towards the touched location
The basic idea is like driving a car. You cannot immediately turn around, but instead must first perform a u-turn. That car must also maintain a constant speed. How might I do this? Bonus question: how can you override whatever method chipmunk calls to update a body's position, and is this a good idea?
There is this tutorial on how to do top down controls using specially configure joints:
http://chipmunk-physics.net/tutorials/ChipmunkTileDemo/
It's based on Chipmunk Pro, but the stuff about controlling the character is easily adapted to vanilla Chipmunk. The "Tank" demo that comes with the non-Pro Chipmunk source implements pretty much the same thing if you want to see some C code for it.
You basically want to rotate the orientation of the player more gradual. You could do this at a constant rate, so when you tap the screen it will start rotating at a constant rate until it has reached the right orientation. This would give a circular turn circle. This will however affect your position, so you would have to keep turning until you would be on a collision course with the position you tapped.
The path you would travel would be similar to that of the game Achtung die kurve.
So you would have to save the location and orientation of the player (x, y and phi coordinates). And to determine whether to stop turning you could do something like this:
dx = playerx - tapx;
dy = playery - tapy;
targetAngle = atan2(dy,dx);
if (phi > targetAngle)
{
if (phi - targetAngle > PI) omega = rotate;
else omega = -rotate;
}
else if (phi < targetAngle)
{
if (targetAngle - phi > PI) omega = -rotate;
else omega = rotate;
}
else omega = 0;
I am implementing a 3D engine for spatial visualisation, and am writing a camera with the following navigation features:
Rotate the camera (ie, analogous to rotating your head)
Rotate around an arbitrary 3D point (a point in space, which is probably not in the center of the screen; the camera needs to rotate around this keeping the same relative look direction, ie the look direction changes too. This does not look directly at the chosen rotation point)
Pan in the camera's plane (so move up/down or left/right in the plane orthogonal to the camera's look vector)
The camera is not supposed to roll - that is, 'up' remains up. Because of this I represent the camera with a location and two angles, rotations around the X and Y axes (Z would be roll.) The view matrix is then recalculated using the camera location and these two angles. This works great for pan and rotating the eye, but not for rotating around an arbitrary point. Instead I get the following behaviour:
The eye itself apparently moving further up or down than it should
The eye not moving up or down at all when m_dRotationX is 0 or pi. (Gimbal lock? How can I avoid this?)
The eye's rotation being inverted (changing the rotation makes it look further up when it should look further down, down when it should look further up) when m_dRotationX is between pi and 2pi.
(a) What is causing this 'drift' in rotation?
This may be gimbal lock. If so, the standard answer to this is 'use quaternions to represent rotation', said many times here on SO (1, 2, 3 for example), but unfortunately without concrete details (example. This is the best answer I've found so far; it's rare.) I've struggled to implemented a camera using quaternions combining the above two types of rotations. I am, in fact, building a quaternion using the two rotations, but a commenter below said there was no reason - it's fine to immediately build the matrix.
This occurs when changing the X and Y rotations (which represent the camera look direction) when rotating around a point, but does not occur simply when directly changing the rotations, i.e. rotating the camera around itself. To me, this doesn't make sense. It's the same values.
(b) Would a different approach (quaternions, for example) be better for this camera? If so, how do I implement all three camera navigation features above?
If a different approach would be better, then please consider providing a concrete implemented example of that approach. (I am using DirectX9 and C++, and the D3DX* library the SDK provides.) In this second case, I will add and award a bounty in a couple of days when I can add one to the question. This might sound like I'm jumping the gun, but I'm low on time and need to implement or solve this quickly (this is a commercial project with a tight deadline.) A detailed answer will also improve the SO archives, because most camera answers I've read so far are light on code.
Thanks for your help :)
Some clarifications
Thanks for the comments and answer so far! I'll try to clarify a few things about the problem:
The view matrix is recalculated from the camera position and the two angles whenever one of those things changes. The matrix itself is never accumulated (i.e. updated) - it is recalculated afresh. However, the camera position and the two angle variables are accumulated (whenever the mouse moves, for example, one or both of the angles will have a small amount added or subtracted, based on the number of pixels the mouse moved up-down and/or left-right onscreen.)
Commenter JCooper states I'm suffering from gimbal lock, and I need to:
add another rotation onto your transform that rotates the eyePos to be
completely in the y-z plane before you apply the transformation, and
then another rotation that moves it back afterward. Rotate around the
y axis by the following angle immediately before and after applying
the yaw-pitch-roll matrix (one of the angles will need to be negated;
trying it out is the fastest way to decide which).
double fixAngle = atan2(oEyeTranslated.z,oEyeTranslated.x);
Unfortunately, when implementing this as described, my eye shoots off above the scene at a very fast rate due to one of the rotations. I'm sure my code is simply a bad implementation of this description, but I still need something more concrete. In general, I find unspecific text descriptions of algorithms are less useful than commented, explained implementations. I am adding a bounty for a concrete, working example that integrates with the code below (i.e. with the other navigation methods, too.) This is because I would like to understand the solution, as well as have something that works, and because I need to implement something that works quickly since I am on a tight deadline.
Please, if you answer with a text description of the algorithm, make sure it is detailed enough to implement ('Rotate around Y, then transform, then rotate back' may make sense to you but lacks the details to know what you mean. Good answers are clear, signposted, will allow others to understand even with a different basis, are 'solid weatherproof information boards.')
In turn, I have tried to be clear describing the problem, and if I can make it clearer please let me know.
My current code
To implement the above three navigation features, in a mouse move event moving based on the pixels the cursor has moved:
// Adjust this to change rotation speed when dragging (units are radians per pixel mouse moves)
// This is both rotating the eye, and rotating around a point
static const double dRotatePixelScale = 0.001;
// Adjust this to change pan speed (units are meters per pixel mouse moves)
static const double dPanPixelScale = 0.15;
switch (m_eCurrentNavigation) {
case ENavigation::eRotatePoint: {
// Rotating around m_oRotateAroundPos
const double dX = (double)(m_oLastMousePos.x - roMousePos.x) * dRotatePixelScale * D3DX_PI;
const double dY = (double)(m_oLastMousePos.y - roMousePos.y) * dRotatePixelScale * D3DX_PI;
// To rotate around the point, translate so the point is at (0,0,0) (this makes the point
// the origin so the eye rotates around the origin), rotate, translate back
// However, the camera is represented as an eye plus two (X and Y) rotation angles
// This needs to keep the same relative rotation.
// Rotate the eye around the point
const D3DXVECTOR3 oEyeTranslated = m_oEyePos - m_oRotateAroundPos;
D3DXMATRIX oRotationMatrix;
D3DXMatrixRotationYawPitchRoll(&oRotationMatrix, dX, dY, 0.0);
D3DXVECTOR4 oEyeRotated;
D3DXVec3Transform(&oEyeRotated, &oEyeTranslated, &oRotationMatrix);
m_oEyePos = D3DXVECTOR3(oEyeRotated.x, oEyeRotated.y, oEyeRotated.z) + m_oRotateAroundPos;
// Increment rotation to keep the same relative look angles
RotateXAxis(dX);
RotateYAxis(dY);
break;
}
case ENavigation::ePanPlane: {
const double dX = (double)(m_oLastMousePos.x - roMousePos.x) * dPanPixelScale;
const double dY = (double)(m_oLastMousePos.y - roMousePos.y) * dPanPixelScale;
m_oEyePos += GetXAxis() * dX; // GetX/YAxis reads from the view matrix, so increments correctly
m_oEyePos += GetYAxis() * -dY; // Inverted compared to screen coords
break;
}
case ENavigation::eRotateEye: {
// Rotate in radians around local (camera not scene space) X and Y axes
const double dX = (double)(m_oLastMousePos.x - roMousePos.x) * dRotatePixelScale * D3DX_PI;
const double dY = (double)(m_oLastMousePos.y - roMousePos.y) * dRotatePixelScale * D3DX_PI;
RotateXAxis(dX);
RotateYAxis(dY);
break;
}
The RotateXAxis and RotateYAxis methods are very simple:
void Camera::RotateXAxis(const double dRadians) {
m_dRotationX += dRadians;
m_dRotationX = fmod(m_dRotationX, 2 * D3DX_PI); // Keep in valid circular range
}
void Camera::RotateYAxis(const double dRadians) {
m_dRotationY += dRadians;
// Limit it so you don't rotate around when looking up and down
m_dRotationY = std::min(m_dRotationY, D3DX_PI * 0.49); // Almost fully up
m_dRotationY = std::max(m_dRotationY, D3DX_PI * -0.49); // Almost fully down
}
And to generate the view matrix from this:
void Camera::UpdateView() const {
const D3DXVECTOR3 oEyePos(GetEyePos());
const D3DXVECTOR3 oUpVector(0.0f, 1.0f, 0.0f); // Keep up "up", always.
// Generate a rotation matrix via a quaternion
D3DXQUATERNION oRotationQuat;
D3DXQuaternionRotationYawPitchRoll(&oRotationQuat, m_dRotationX, m_dRotationY, 0.0);
D3DXMATRIX oRotationMatrix;
D3DXMatrixRotationQuaternion(&oRotationMatrix, &oRotationQuat);
// Generate view matrix by looking at a point 1 unit ahead of the eye (transformed by the above
// rotation)
D3DXVECTOR3 oForward(0.0, 0.0, 1.0);
D3DXVECTOR4 oForward4;
D3DXVec3Transform(&oForward4, &oForward, &oRotationMatrix);
D3DXVECTOR3 oTarget = oEyePos + D3DXVECTOR3(oForward4.x, oForward4.y, oForward4.z); // eye pos + look vector = look target position
D3DXMatrixLookAtLH(&m_oViewMatrix, &oEyePos, &oTarget, &oUpVector);
}
It seems to me that "Roll" shouldn't be possible given the way you form your view matrix. Regardless of all the other code (some of which does look a little funny), the call D3DXMatrixLookAtLH(&m_oViewMatrix, &oEyePos, &oTarget, &oUpVector); should create a matrix without roll when given [0,1,0] as an 'Up' vector unless oTarget-oEyePos happens to be parallel to the up vector. This doesn't seem to be the case since you're restricting m_dRotationY to be within (-.49pi,+.49pi).
Perhaps you can clarify how you know that 'roll' is happening. Do you have a ground plane and the horizon line of that ground plane is departing from horizontal?
As an aside, in UpdateView, the D3DXQuaternionRotationYawPitchRoll seems completely unnecessary since you immediately turn around and change it into a matrix. Just use D3DXMatrixRotationYawPitchRoll as you did in the mouse event. Quaternions are used in cameras because they're a convenient way to accumulate rotations happening in eye coordinates. Since you're only using two axes of rotation in a strict order, your way of accumulating angles should be fine. The vector transformation of (0,0,1) isn't really necessary either. The oRotationMatrix should already have those values in the (_31,_32,_33) entries.
Update
Given that it's not roll, here's the problem: you create a rotation matrix to move the eye in world coordinates, but you want the pitch to happen in camera coordinates. Since roll isn't allowed and yaw is performed last, yaw is always the same in both the world and camera frames of reference. Consider the images below:
Your code works fine for local pitch and yaw because those are accomplished in camera coordinates.
But when you rotate around a reference point, you are creating a rotation matrix that is in world coordinates and using that to rotate the camera center. This works okay if the camera's coordinate system happens to line up with the world's. However, if you don't check to see if you're up against the pitch limit before you rotate the camera position, you will get crazy behavior when you hit that limit. The camera will suddenly start to skate around the world--still 'rotating' around the reference point, but no longer changing orientation.
If the camera's axes don't line up with the world's, strange things will happen. In the extreme case, the camera won't move at all because you're trying to make it roll.
The above is what would normally happen, but since you handle the camera orientation separately, the camera doesn't actually roll.
Instead, it stays upright, but you get strange translation going on.
One way to handle this would be to (1)always put the camera into a canonical position and orientation relative to the reference point, (2)make your rotation, and then (3)put it back when you're done (e.g., similar to the way that you translate the reference point to the origin, apply the Yaw-Pitch rotation, and then translate back). Thinking more about it, however, this probably isn't the best way to go.
Update 2
I think that Generic Human's answer is probably the best. The question remains as to how much pitch should be applied if the rotation is off-axis, but for now, we'll ignore that. Maybe it'll give you acceptable results.
The essence of the answer is this: Before mouse movement, your camera is at c1 = m_oEyePos and being oriented by M1 = D3DXMatrixRotationYawPitchRoll(&M_1,m_dRotationX,m_dRotationY,0). Consider the reference point a = m_oRotateAroundPos. From the point of view of the camera, this point is a'=M1(a-c1).
You want to change the orientation of the camera to M2 = D3DXMatrixRotationYawPitchRoll(&M_2,m_dRotationX+dX,m_dRotationY+dY,0). [Important: Since you won't allow m_dRotationY to fall outside of a specific range, you should make sure that dY doesn't violate that constraint.] As the camera changes orientation, you also want its position to rotate around a to a new point c2. This means that a won't change from the perspective of the camera. I.e., M1(a-c1)==M2(a-c2).
So we solve for c2 (remember that the transpose of a rotation matrix is the same as the inverse):
M2TM1(a-c1)==(a-c2) =>
-M2TM1(a-c1)+a==c2
Now if we look at this as a transformation being applied to c1, then we can see that it is first negated, then translated by a, then rotated by M1, then rotated by M2T, negated again, and then translated by a again. These are transformations that graphics libraries are good at and they can all be squished into a single transformation matrix.
#Generic Human deserves credit for the answer, but here's code for it. Of course, you need to implement the function to validate a change in pitch before it's applied, but that's simple. This code probably has a couple typos since I haven't tried to compile:
case ENavigation::eRotatePoint: {
const double dX = (double)(m_oLastMousePos.x - roMousePos.x) * dRotatePixelScale * D3DX_PI;
double dY = (double)(m_oLastMousePos.y - roMousePos.y) * dRotatePixelScale * D3DX_PI;
dY = validatePitch(dY); // dY needs to be kept within bounds so that m_dRotationY is within bounds
D3DXMATRIX oRotationMatrix1; // The camera orientation before mouse-change
D3DXMatrixRotationYawPitchRoll(&oRotationMatrix1, m_dRotationX, m_dRotationY, 0.0);
D3DXMATRIX oRotationMatrix2; // The camera orientation after mouse-change
D3DXMatrixRotationYawPitchRoll(&oRotationMatrix2, m_dRotationX + dX, m_dRotationY + dY, 0.0);
D3DXMATRIX oRotationMatrix2Inv; // The inverse of the orientation
D3DXMatrixTranspose(&oRotationMatrix2Inv,&oRotationMatrix2); // Transpose is the same in this case
D3DXMATRIX oScaleMatrix; // Negative scaling matrix for negating the translation
D3DXMatrixScaling(&oScaleMatrix,-1,-1,-1);
D3DXMATRIX oTranslationMatrix; // Translation by the reference point
D3DXMatrixTranslation(&oTranslationMatrix,
m_oRotateAroundPos.x,m_oRotateAroundPos.y,m_oRotateAroundPos.z);
D3DXMATRIX oTransformMatrix; // The full transform for the eyePos.
// We assume the matrix multiply protects against variable aliasing
D3DXMatrixMultiply(&oTransformMatrix,&oScaleMatrix,&oTranslationMatrix);
D3DXMatrixMultiply(&oTransformMatrix,&oTransformMatrix,&oRotationMatrix1);
D3DXMatrixMultiply(&oTransformMatrix,&oTransformMatrix,&oRotationMatrix2Inv);
D3DXMatrixMultiply(&oTransformMatrix,&oTransformMatrix,&oScaleMatrix);
D3DXMatrixMultiply(&oTransformMatrix,&oTransformMatrix,&oTranslationMatrix);
D3DXVECTOR4 oEyeFinal;
D3DXVec3Transform(&oEyeFinal, &m_oEyePos, &oTransformMatrix);
m_oEyePos = D3DXVECTOR3(oEyeFinal.x, oEyeFinal.y, oEyeFinal.z)
// Increment rotation to keep the same relative look angles
RotateXAxis(dX);
RotateYAxis(dY);
break;
}
I think there is a much simpler solution that lets you sidestep all rotation issues.
Notation: A is the point we want to rotate around, C is the original camera location, M is the original camera rotation matrix that maps global coordinates to the camera's local viewport.
Make a note of the local coordinates of A, which are equal to A' = M × (A - C).
Rotate the camera like you would in normal "eye rotation" mode. Update the view matrix M so that it is modified to M2 and C remains unchanged.
Now we would like to find C2 such that A' = M2 × (A - C2).
This is easily done by the equation C2 = A - M2-1 × A'.
Voilà, the camera has been rotated and because the local coordinates of A are unchanged, A remains at the same location and the same scale and distance.
As an added bonus, the rotation behavior is now consistent between "eye rotation" and "point rotation" mode.
You rotate around the point by repeatedly applying small rotation matrices, this probably cause the drift (small precision errors add up) and I bet you will not really do a perfect circle after some time. Since the angles for the view use simple 1-dimension double, they have much less drift.
A possible fix would be to store a dedicated yaw/pitch and relative position from the point when you enter that view mode, and using those to do the math. This requires a bit more bookkeeping, since you need to update those when moving the camera. Note that it will also make the camera move if the point move, which I think is an improvement.
If I understand correctly, you are satisfied with the rotation component in the final matrix (save for inverted rotation controls in the problem #3), but not with the translation part, is that so?
The problem seems to come from the fact that you treating them differently: you are recalculating the rotation part from scratch every time, but accumulate the translation part (m_oEyePos). Other comments mention precision problems, but it's actually more significant than just FP precision: accumulating rotations from small yaw/pitch values is simply not the same---mathematically---as making one big rotation from the accumulated yaw/pitch. Hence the rotation/translation discrepancy. To fix this, try recalculating eye position from scratch simultaneously with the rotation part, similarly to how you find "oTarget = oEyePos + ...":
oEyePos = m_oRotateAroundPos - dist * D3DXVECTOR3(oForward4.x, oForward4.y, oForward4.z)
dist can be fixed or calculated from the old eye position. That will keep the rotation point in the screen center; in the more general case (which you are interested in), -dist * oForward here should be replaced by the old/initial m_oEyePos - m_oRotateAroundPos multiplied by the old/initial camera rotation to bring it to the camera space (finding a constant offset vector in camera's coordinate system), then multiplied by the inverted new camera rotation to get the new direction in the world.
This will, of course, be subject to gimbal lock when the pitch is straight up or down. You'll need to define precisely what behavior you expect in these cases to solve this part. On the other hand, locking at m_dRotationX=0 or =pi is rather strange (this is yaw, not pitch, right?) and might be related to the above.
Please refer to the video at
http://www.youtube.com/watch?v=_DyzwZJaDfM
The "brown" body is controlled with mouse and when mouse is pressed I calculate force using Hookes law (refereed to http://www.box2d.org/forum/viewtopic.php?f=4&t=116 ) and the "blue" body should attract to the "brown" body.
But as seen in the video,"blue" body keep orbiting around and doesn't come stop.What I wanted to implement is "elastic rope" like thing.
First I tried using DistanceJoint ,but I cannot give a static distance to the joint.
here is my implementation for hookes law -
-(void)applyHookesLaw:(b2Body*)bodyA:(b2Body*)bodyB:(float) k:(float) friction:(float)desiredDist
{
b2Vec2 pA=bodyA->GetPosition();
b2Vec2 pB=bodyB->GetPosition();
b2Vec2 diff=pB- pA;
b2Vec2 vA=bodyA->GetLinearVelocity();
b2Vec2 vB=bodyB->GetLinearVelocity();
b2Vec2 vdiff=vB-vA;
float dx=diff.Normalize();
float vrel=vdiff.x * diff.x + vdiff.y * diff.y;
float forceMag= -k*(dx-desiredDist);//-friction*vrel;
diff*=forceMag;
bodyA->ApplyForce(-1*diff,bodyB->GetPosition());
//bodyA->wakeUp()
}
Any tips please?
PS - gravity of the world is 0.0
Hooke's law when incorporated into Newton's Second law is a second order differential equation: m d^2 x/dt^2 = - k x, where x is is a vector. As Beta points out in the comments, you can just add friction. Absent a friction term, orbits like you observe are common, and they will continue indefinitely. The usual way add friction is to add a term that is proportional to velocity, and like the Hookean term (-k*x), it is also negative, i.e. it opposes the motion.
If I'm reading your code correctly, you already have something like that term in the comments following setting forceMag. But, I don't understand your calculation of vrel, it looks like the dot product between the relative velocity and the vector joining the two bodies. vdiff is already the correct form for this. Also, unlike the spring force, this force is directed along the relative velocity (vdiff). So, to implement it I'd change the line where you call ApplyForce on bodyA to
bodyA->ApplyForce(-1*diff - friction*vdiff,bodyB->GetPosition());