How to properly apply torque or angular impulse to body in PlayN? - playn

I've been trying to apply an angular impulse to a Body object in PlayN, but to no avail. Whichever value (radials) I enter, the angle of the body never changes. I have tried to set the torque as well with no results.
Example code that doesn't work:
BodyDef def = new BodyDef();
def.type = BodyType.DYNAMIC;
Body body = world.createBody(def);
float degToRad = (float) (180 / Math.PI);
float radials = (float) (50 / degToRad);
// None of the following options work.
body.applyAngularImpulse(radials); // Immediate angular change.
body.applyTorque(radials); // Angular change over time.
How can I get a valid body object to change its angle without manually setting its angular velocity (e.g. with setAngularVelocity)?
Thanks in advance!
I did notice that the torque and angular velocity are reset by calling the setWake method, which I never do manually, but it is invoked by the Island class:
public void setAwake(boolean flag) {
...
m_angularVelocity = 0.0f;
m_torque = 0.0f;
...
}
Note: Setting the angular velocity is not an option because I rely on the physics simulation. I've found an article for Box2D angle rotation, but it didn't change the outcome of the applyAngularImpulse method.

I've debugged some, and come to the conclusion the values that I've been using were way too low. When going through the applyAngularImpulse code I noticed a few things:
public void applyAngularImpulse(float impulse) {
if (m_type != BodyType.DYNAMIC) {
return;
}
if (isAwake() == false) {
setAwake(true);
}
m_angularVelocity += m_invI * impulse;
}
m_invI is a really low value, something like 2.44882598E-4 so it makes sense if I use a too low value, that there is just no visible effect. If calculate the impulse from the question's example multiplied by the m_invI field:
m_invI = 0.00044882598 = 2.449E-4
impulse = 50 / 180 / PI = 0.08841941282 = 8.842E-2
m_invI * impulse = 2.449E-4 * 8.842E-2 = 0.00002165237 = 2.165E-5 <--
That means just an angular velocity increment of 0.00002165237 per update cycle... Which is way too low to be noticeable it seems.
Also, while browsing the Box2D forums for similar situations, I found it there's a best practice to include the body's inertia into your angular impulse calculation. Like this:
body.applyAngularImpulse(speedValue * body.getInertia());
The inertia tends to be a high value (+4000) for me.
Hope this can be some help to others that have trouble applying an angular impulse or torque.

In my case I forgot to call
body.setFixedRotation(false);
Even though I was setting mass, an inertia of 0 was reported and all torques and angular impulses were ignored.
Apparently it is fixed by default.

I had the same problem.
In my case I accidentally put my points in CW order (leading to negative area and zero mass). This was my incorrect code:
// incorrect version
triangleVertices.push(new B2Vec2(width / 2, 0.0)); // top
triangleVertices.push(new B2Vec2(0.0, height)); // left
triangleVertices.push(new B2Vec2(width, heightM)); // right
That looks like CCW, right? Nope, not in my coordinate system (+Y is down).
// corrected version
triangleVertices.push(new B2Vec2(width / 2, 0.0)); // top
triangleVertices.push(new B2Vec2(width, heightM)); // right
triangleVertices.push(new B2Vec2(0.0, height)); // left

Related

How to update camera up vector when I rotate it?

I am willing to make an FPS "fly-mode" THREE.js application (camera yaw and pitch angles are modified whenever the mouse is moved). I also want to have it have WASD controls, with A and D being used to strafe left and right respectively. This is my code so far:
onkeydown(event) {
var distance = 1.1;
var v;
const vecFront = this.camera.getWorldDirection();
switch (event.key) {
case "w":
v = vecFront.multiplyScalar(distance);
break;
case "s":
v = vecFront.multiplyScalar(-distance);
break;
case "a":
// don't know
break;
case "d":
// don't know
break;
}
this.camera.position.add(v);
}
For going forward and backward, the code works fine. For the left and right vectors I think camera.up comes into play, but I don't know how. If you provide code, please explain everything in detail (geometrically). Thank you in advance for your help.
P.S.: I know there are already some THREE.js files that do this, but the point is for me to learn some basic geometry.
The right vector R is given by the vector cross-product of the world direction (W) and the up vector (U):
R = (W.y * U.z - W.z * U.y, W.z * U.x - W.x * U.z, W.x * U.y - W.y * U.x)
Remember to normalize this vector before you use it.
However, there is a more efficient way, using the view matrix of the camera (given by the matrixWorldInverse field). The (already normalized too!) right vector is given by the first three elements of the first row of the matrix.

Calculating bank angle between two objects

I have a drone following a path for movement. That is, it doesn't use a rigidbody so I don't have access to velocity or magnitude and such. It follows the path just fine, but I would like to add banking to it when it turns left or right. I use a dummy object in front of the drone, thinking I could calculate the bank/tilt amount using the transform vectors from the two objects.
I've been working on this for days as I don't have a lot of math skills. Basically I've been copying pieces of code trying to get things to work. Nothing I do works to make the drone bank. The following code manages to spin (not bank).
// Update is called once per frame
void Update () {
Quaternion rotation = Quaternion.identity;
Vector3 dir = (dummyObject.transform.position - this.transform.position).normalized;
float angle = Vector3.Angle( dir, transform.up );
float rollAngle = CalculateRollAngle(angle);
rotation.SetLookRotation(dir, transform.right);// + rollIntensity * smoothRoll * right);
rotation *= Quaternion.Euler(new Vector3(0, 0, rollAngle));
transform.rotation = rotation;
}
/// <summary>
/// Calculates Roll and smoothes it (to compensates for non C2 continuous control points algorithm) /// </summary>
/// <returns>The roll angle.</returns>
/// <param name="rollFactor">Roll factor.</param>
float CalculateRollAngle(float rollFactor)
{
smoothRoll = Mathf.Lerp(smoothRoll, rollFactor, rollSmoothing * Time.deltaTime);
float angle = Mathf.Atan2(1, smoothRoll * rollIntensity);
angle *= Mathf.Rad2Deg;
angle -= 90;
TurnRollAngle = angle;
angle += RollOffset;
return angle;
}
Assuming you have waypoints the drone is following, you should figure out the angle between the last two (i.e. your "now-facing" and "will be facing" directions). The easy way is to use Vector2.Angle.
I would use this angle to determine the amount I'll tilt the drone's body: the sharper the turn, the harder the banking. I would use a ratio value (public initially so I can manipulate it from the editor).
Next, instead of doing any math I would rely on the engine to do the rotation for me - so I would go for Transform.Rotate function.In case banking can go too high and look silly, I would set a maximum for that and Clamp my calculated banking angle between zero and max.
Without knowing exactly what you do and how, it's not easy to give perfect code, but for a better understand of the above, here's some (untested, i.e. pseudo) code for the solution I visualize:
public float turnSpeed = 7.0f; //the drone will "rotate toward the new waypoint" by this speed
//bankSpeed+turnBankRatio must be two times "faster" (and/or smaller degree) than turning, see details in 'EDIT' as of why:
public float bankSpeed = 14.0f; //banking speed
public float turnBankRatio = .5f; //90 degree turn == 45 degree banking
private float turnAngle = 0.0f; //this is the 'x' degree turning angle we'll "Lerp"
private float turnAngleABS = 0.0f; //same as turnAngle but it's an absolute value. Storing to avoid Mathf.Abs() in Update()!
private float bankAngle = 0.0f; //banking degree
private bool isTurning = false; //are we turning right now?
//when the action is fired for the drone it should go for the next waypoint, call this guy
private void TurningTrigger() {
//remove this line after testing, it's some extra safety
if (isTurning) { Debug.LogError("oups! must not be possible!"); return; }
Vector2 droneOLD2DAngle = GetGO2DPos(transform.position);
//do the code you do for the turning/rotation of drone here!
//or use the next waypoint's .position as the new angle if you are OK
//with the snippet doing the turning for you along with banking. then:
Vector2 droneNEW2DAngle = GetGO2DPos(transform.position);
turnAngle = Vector2.Angle(droneOLD2DAngle, droneNEW2DAngle); //turn degree
turnAngleABS = Mathf.Abs(turnAngle); //avoiding Mathf.Abs() in Update()
bankAngle = turnAngle * turnBankRatio; //bank angle
//you can remove this after testing. This is to make sure banking can
//do a full run before the drone hits the next waypoint!
if ((turnAngle * turnSpeed) < (bankAngle * bankSpeed)) {
Debug.LogError("Banking degree too high, or banking speed too low to complete maneuver!");
}
//you can clamp or set turnAngle based on a min/max here
isTurning = true; //all values were set, turning and banking can start!
}
//get 2D position of a GO (simplified)
private Vector2 GetGO2DPos(Vector3 worldPos) {
return new Vector2(worldPos.x, worldPos.z);
}
private void Update() {
if (isTurning) {
//assuming the drone is banking to the "side" and "side" only
transform.Rotate(0, 0, bankAngle * time.deltaTime * bankSpeed, Space.Self); //banking
//if the drone is facing the next waypoint already, set
//isTurning to false
} else if (turnAngleABS > 0.0f) {
//reset back to original position (with same speed as above)
//at least "normal speed" is a must, otherwise drone might hit the
//next waypoint before the banking reset can finish!
float bankAngle_delta = bankAngle * time.deltaTime * bankSpeed;
transform.Rotate(0, 0, -1 * bankAngle_delta, Space.Self);
turnAngleABS -= (bankAngle_delta > 0.0f) &#63 bankAngle_delta : -1 * bankAngle_delta;
}
//the banking was probably not set back to exactly 0, as time.deltaTime
//is not a fixed value. if this happened and looks ugly, reset
//drone's "z" to Quaternion.identity.z. if it also looks ugly,
//you need to test if you don't """over bank""" in the above code
//by comparing bankAngle_delta + 'calculated banking angle' against
//the identity.z value, and reset bankAngle_delta if it's too high/low.
//when you are done, your turning animation is over, so:
}
Again, this code might not perfectly fit your needs (or compile :P), so focus on the idea and the approach, not the code itself. Sorry for not being able right now to put something together and test myself - but I hope I helped. Cheers!
EDIT: Instead of a wall of text I tried to answer your question in code (still not perfect, but goal is not doing the job, but to help with some snippets and ideas :)
So. Basically, what you have is a distance and "angle" between two waypoints. This distance and your drone's flight/walk/whatever speed (which I don't know) is the maximum amount of time available for:
1. Turning, so the drone will face in the new direction
2. Banking to the side, and back to zero/"normal"
As there's two times more action on banking side, it either has to be done faster (bankSpeed), or in a smaller angle (turnBankRatio), or both, depending on what looks nice and feels real, what your preference is, etc. So it's 100% subjective. It's also your call if the drone turns+banks quickly and approaches toward the next waypoint, or does things in slow pace and turns just a little if has a lot of time/distance and does things fast only if it has to.
As of isTurning:
You set it to true when the drone reached a waypoint and heads out to the next one AND the variables to (turn and) bank were set properly. When you set it to false? It's up to you, but the goal is to do so when the maneuver is finished (this was buggy in the snippet the first time as this "optimal status" was not possible to ever be reached) so he drone can "reset banking".For further details on what's going on, see code comments.Again, this is just a snippet to support you with a possible solution for your problem. Give it some time and understand what's going on. It really is easy, you just need some time to cope ;)Hope this helps! Enjoy and cheers! :)

how to color point cloud from image pixels?

I am using google tango tablet to acquire point cloud data and RGB camera images. I want to create 3D scan of the room. For that i need to map 2D image pixels to point cloud point. I will be doing this with a lot of point clouds and corresponding images.Thus I need to write a code script which has two inputs 1. point cloud and 2. image taken from the same point in same direction and the script should output colored point cloud. How should i approach this & which platforms will be very simple to use?
Here is the math to map a 3D point v to 2D pixel space in the camera image (assuming that v already incorporates the extrinsic camera position and orientation, see note at bottom*):
// Project to tangent space.
vec2 imageCoords = v.xy/v.z;
// Apply radial distortion.
float r2 = dot(imageCoords, imageCoords);
float r4 = r2*r2;
float r6 = r2*r4;
imageCoords *= 1.0 + k1*r2 + k2*r4 + k3*r6;
// Map to pixel space.
vec3 pixelCoords = cameraTransform*vec3(imageCoords, 1);
Where cameraTransform is the 3x3 matrix:
[ fx 0 cx ]
[ 0 fy cy ]
[ 0 0 1 ]
with fx, fy, cx, cy, k1, k2, k3 from TangoCameraIntrinsics.
pixelCoords is declared vec3 but is actually 2D in homogeneous coordinates. The third coordinate is always 1 and so can be ignored for practical purposes.
Note that if you want texture coordinates instead of pixel coordinates, that is just another linear transform that can be premultiplied onto cameraTransform ahead of time (as is any top-to-bottom vs. bottom-to-top scanline addressing).
As for what "platform" (which I loosely interpreted as "language") is simplest, the native API seems to be the most straightforward way to get your hands on camera pixels, though it appears people have also succeeded with Unity and Java.
* Points delivered by TangoXYZij already incorporate the depth camera extrinsic transform. Technically, because the current developer tablet shares the same hardware between depth and color image acquisition, you won't be able to get a color image that exactly matches unless both your device and your scene are stationary. Fortunately in practice, most applications can probably assume that neither the camera pose nor the scene changes enough in one frame time to significantly affect color lookup.
This answer is not original, it is simply meant as a convenience for Unity users who would like the correct answer, as provided by #rhashimoto, worked out for them. My contribution (hopefully) is providing code that reduces the normal 16 multiplies and 12 adds (given Unity only does 4x4 matrices) to 2 multiplies and 2 adds by dropping out all of the zero results. I ran a little under a million points through the test, checking each time that my calculations agreed with the basic matrix calculations - defined as the absolute difference between the two results being less than machine epsilon - I'm as comfortable with this as I can be knowing that #rhashimoto may show up and poke a giant hole in it :-)
If you want to switch back and forth, remember this is C#, so the USEMATRIXMATH define must appear at the beginning of the file.
Given there's only one Tango device right now, and I'm assuming the intrinsics are constant across all of the devices, I just dumped them in as constants, such that
fx = 1042.73999023438
fy = 1042.96997070313
cx = 637.273986816406
cy = 352.928985595703
k1 = 0.228532999753952
k2 = -0.663019001483917
k3 = 0.642908990383148
Yes they can be dumped in as constants, which would make things more readable, and C# is probably smart enough to optimize it out - however, I spent too much of my life in Agner Fogg's stuff, and will always be paranoid.
The commented out code at the bottom is for testing the difference, should you desire. You'll have to uncomment some other stuff, and comment out the returns if you want to test the results.
My thanks again to #rhashimoto, this is far far better than what I had
I have stayed true to his logic, remember these are pixel coordinates, not UV coordinates - he is correct that you can premultiply the transform to get normalized UV values, but since he schooled me on this once already, I will stick with exactly the math he presented before I fiddle with too much :-)
static public Vector2 PictureUV(Vector3 tangoDepthPoint)
{
Vector2 imageCoords = new Vector2(tangoDepthPoint.x / tangoDepthPoint.z, tangoDepthPoint.y / tangoDepthPoint.z);
float r2 = Vector2.Dot(imageCoords, imageCoords);
float r4 = r2*r2;
float r6 = r2*r4;
imageCoords *= 1.0f + 0.228532999753952f*r2 + -0.663019001483917f*r4 + 0.642908990383148f*r6;
Vector3 ic3 = new Vector3(imageCoords.x,imageCoords.y,1);
#if USEMATRIXMATH
Matrix4x4 cameraTransform = new Matrix4x4();
cameraTransform.SetRow(0,new Vector4(1042.73999023438f,0,637.273986816406f,0));
cameraTransform.SetRow(1, new Vector4(0, 1042.96997070313f, 352.928985595703f, 0));
cameraTransform.SetRow(2, new Vector4(0, 0, 1, 0));
cameraTransform.SetRow(3, new Vector4(0, 0, 0, 1));
Vector3 pixelCoords = cameraTransform * ic3;
return new Vector2(pixelCoords.x, pixelCoords.y);
#else
//float v1 = 1042.73999023438f * imageCoords.x + 637.273986816406f;
//float v2 = 1042.96997070313f * imageCoords.y + 352.928985595703f;
//float v3 = 1;
return new Vector2(1042.73999023438f * imageCoords.x + 637.273986816406f,1042.96997070313f * imageCoords.y + 352.928985595703);
#endif
//float dx = Math.Abs(v1 - pixelCoords.x);
//float dy = Math.Abs(v2 - pixelCoords.y);
//float dz = Math.Abs(v3 - pixelCoords.z);
//if (dx > float.Epsilon || dy > float.Epsilon || dz > float.Epsilon)
// UnityEngine.Debug.Log("Well, that didn't work");
//return new Vector2(v1, v2);
}
As one final note, do note the code he provided is GLSL - if you're just using this for pretty pictures, use it - this is for those that actually need to perform additional processing.

Potential floating point issue with cosine acceleration curve

I am using a cosine curve to apply a force on an object between the range [0, pi]. By my calculations, that should give me a sine curve for the velocity which, at t=pi/2 should have a velocity of 1.0f
However, for the simplest of examples, I get a top speed of 0.753.
Now if this is a floating point issue, that is fine, but that is a very significant error so I am having trouble accepting that it is (and if it is, why is there such a huge error computing these values).
Some code:
// the function that gives the force to apply (totalTime = pi, maxForce = 1.0 in this example)
return ((Mathf.Cos(time * (Mathf.PI / totalTime)) * maxForce));
// the engine stores this value and in the next fixed update applies it to the rigidbody
// the mass is 1 so isn't affecting the result
engine.ApplyAccelerateForce(applyingForce * ship.rigidbody2D.mass);
Update
There is no gravity being applied to the object, no other objects in the world for it to interact with and no drag. I'm also using a RigidBody2D so the object is only moving on the plane.
Update 2
Ok have tried a super simple example and I get the result I am expecting so there must be something in my code. Will update once I have isolated what is different.
For the record, super simple code:
float forceThisFrame;
float startTime;
// Use this for initialization
void Start () {
forceThisFrame = 0.0f;
startTime = Time.fixedTime;
}
// Update is called once per frame
void Update () {
float time = Time.fixedTime - startTime;
if(time <= Mathf.PI)
{
forceThisFrame = Mathf.Cos (time);
if(time >= (Mathf.PI /2.0f)- 0.01f && time <= (Mathf.PI /2.0f) + 0.01f)
{
print ("Speed: " + rigidbody2D.velocity);
}
}
else
{
forceThisFrame = 0.0f;
}
}
void FixedUpdate()
{
rigidbody2D.AddForce(forceThisFrame * Vector2.up);
}
Update 3
I have changed my original code to match the above example as near as I can (remaining differences listed below) and I still get the discrepancy.
Here are my results of velocity against time. Neither of them make sense to me, with a constant force of 1N, that should result in a linear velocity function v(t) = t but that isn't quite what is produced by either example.
Remaining differences:
The code that is "calculating" the force (now just returning 1) is being run via a non-unity DLL, though the code itself resides within a Unity DLL (can explain more but can't believe this is relevant!)
The behaviour that is applying the force to the rigid body is a separate behaviour.
One is moving a cube in an empty enviroment, the other is moving a Model3D and there is a plane nearby - tried a cube with same code in broken project, same problem
Other than that, I can't see any difference and I certainly can't see why any of those things would affect it. They both apply a force of 1 on an object every fixed update.
For the cosine case this isn't a floating point issue, per se, it's an integration issue.
[In your 'fixed' acceleration case there are clearly also minor floating point issues].
Obviously acceleration is proportional to force (F = ma) but you can't just simply add the acceleration to get the velocity, especially if the time interval between frames is not constant.
Simplifying things by assuming that the inter-frame acceleration is constant, and therefore following v = u + at (or alternately ∂v = a.∂t) you need to scale the effect of the acceleration in proportion to the time elapsed since the last frame. It follows that the smaller ∂t is, the more accurate your integration.
This was a multi-part problem that started with me not fully understanding Update vs. FixedUpdate in Unity, see this question on GameDev.SE for more info on that part.
My "fix" from that was advancing a timer that went with the fixed update so as to not apply the force wrong. The problem, as demonstrated by Eric Postpischil was because the FixedUpdate, despite its name, is not called every 0.02s but instead at most every 0.02s. The fix for this was, in my update to apply some scaling to the force to apply to accomodate for missed fixed updates. My code ended up looking something like:
Called From Update
float oldTime = time;
time = Time.fixedTime - startTime;
float variableFixedDeltaTime = time - oldTime;
float fixedRatio = variableFixedDeltaTime / Time.fixedDeltaTime;
if(time <= totalTime)
{
applyingForce = forceFunction.GetValue(time) * fixedRatio;
Vector2 currentVelocity = ship.rigidbody2D.velocity;
Vector2 direction = new Vector2(ship.transform.right.x, ship.transform.right.y);
float velocityAlongDir = Vector2.Dot(currentVelocity, direction);
float velocityPrediction = velocityAlongDir + (applyingForce * lDeltaTime);
if(time > 0.0f && // we are not interested if we are just starting
((velocityPrediction < 0.0f && velocityAlongDir > 0.0f ) ||
(velocityPrediction > 0.0f && velocityAlongDir < 0.0f ) ))
{
float ratio = Mathf.Abs((velocityAlongDir / (applyingForce * lDeltaTime)));
applyingForce = applyingForce * ratio;
// We have reversed the direction so we must have arrived
Deactivate();
}
engine.ApplyAccelerateForce(applyingForce);
}
Where ApplyAccelerateForce does:
public void ApplyAccelerateForce(float requestedForce)
{
forceToApply += requestedForce;
}
Called from FixedUpdate
rigidbody2D.AddForce(forceToApply * new Vector2(transform.right.x, transform.right.y));
forceToApply = 0.0f;

Cocos2D/Math - clean angle conversion

Mornin' SO!
I'm just trying to hone my math-fu, and I have some questions regarding Cocos2D in particular. Since Cocos2D wants to 'simplify' things, all sprites have a rotation property, ranging from 0-360 (359?) CW. This forces you to do some rather (for me) mind-humping conversions when dealing with functions like atan.
So f.ex. this method:
- (void)rotateTowardsPoint:(CGPoint)point
{
// vector from me to the point
CGPoint v = ccpSub(self.position, point);
// ccpToAngle is just a cute wrapper for atan2f
// the macro is self explanatory and the - is to flip the direction I guess
float angle = -CC_RADIANS_TO_DEGREES(ccpToAngle(v));
// just to get it all in the range of 0-360
if(angle < 0.f)
angle += 360.0f;
// but since '0' means east in Cocos..
angle += 180.0f;
// get us in the range of 0-360 again
if(angle > 360.0f)
angle -= 360.0f;
self.rotation = angle;
}
works as intended. But to me it looks kind of brute forced. Is there a cleaner way to achieve the same effect?
It is enough to do
float angle = -CC_RADIANS_TO_DEGREES(ccpToAngle(v));
self.rotation = angle + 180.0f;
for equivalent transformations
// vector from me to the point
CGPoint v = ccpSub(self.position, point);
actually, that's vector from point to you.
// just to get it all in the range of 0-360
you don't need to do that.

Resources