Converting ISO 6709 Formatted GPS Coordinates to Decimal Degrees in R - r

We have a piece of equipment that outputs its GPS coordinates as numeric values to file in an ISO 6709 lat/lon format of Lat = ±DDMM.MMMM & Lon = ±DDDMM.MMMM
Are there any packages with functions (or custom functions) in R that will convert this to a Decimal degrees format? (ie: ±DD.DDDDDD & ±DDD.DDDDDD)
An example would be that lat & lon (2433.056, -8148.443) would be converted to (24.55094, -81.80739).

You could read in the values from the file using something like read.csv or read.delim.
Then to convert from DDMM.MMMM and DDDMM.MMMM you could use something like this (of course modify as needed for the form of your input/outputs):
convertISO6709 <- function( lat, lon ) {
# will just do lat and lon together, as the process is the same for both
# It's simpler to do the arithmetic on positive numbers, we'll add the signs
# back in at the end.
latlon <- c(lat,lon)
sgns <- sign(latlon)
latlon <- abs(latlon)
# grab the MM.MMMM bit, which is always <100. '%%' is modular arithmetic.
mm <- latlon %% 100
# grab the DD bit. Divide by 100 because of the MM.MMMM bit.
dd <- (latlon - mm)/100
# convert to decimal degrees, don't forget to add the signs back!
out_latlon <- (dd+mm/60) * sgns
return(out_latlon)
}

This may not be the most elegant PHP code, but it does work:
function convertISO6709($coord)
{
$abs_coord = abs($coord);
$sign = ($abs_coord == $coord) ? 1 : -1;
$m = 2;
$dlen = 2;
$s = 0;
$seconds = 0;
switch (strlen($abs_coord)) {
case 4 :
break;
case 5 :
$dlen = 3;
$m = 3;
break;
case 6 :
$s = 4;
break;
}
$degrees = substr($abs_coord, 0, $dlen);
$minutes = substr($abs_coord, $m, 2);
if ($s != 0) {
$seconds = substr($abs_coord, $s, 2);
}
return ($degrees + ($minutes / 60) + ($seconds / 3600)) * $sign;
}

I has a similar issue getting coordinates from FedEx WS. I used this function to get the values from a string like +19.467945-99.14357/:
function convertCoordISO6709($coord)
{
//$coord example
//$coord = +19.467945-99.14357/
$returnArray[0] = 1;//result non 0 means error
$returnArray[1] = 0;//Lat
$returnArray[2] = 0;//long
$coord = trim($coord,"/"); //Strip / sign
//look for + - sign
$lat_sign = substr($coord,0,1); //strip and save the first sign (latitude value)
$sub_coord = substr($coord,1,strlen($coord));
if(count(explode("+",$sub_coord)) == 2) //Second value is + for the longitude
{
$coords=explode("+",$sub_coord);
$returnArray[0] = 0;
$returnArray[1] = $lat_sign.$coords[0];
$returnArray[2] = "+".$coords[1];
}
else //Second value is - for the longitude
{
$coords=explode("-",$sub_coord);
$returnArray[0] = 0;
$returnArray[1] = $lat_sign.$coords[0];
$returnArray[2] = "-".$coords[1];
}
return $returnArray;
}

Related

Octave: How can I plot unknown length serial data using the instrument-control package?

I found this example which sorta works... but it's not perfect. For starters it plots 10 at once but that's an easy fix. Problem is that it does not dynamically get line endings and instead relies on byte count.
pkg load instrument-control
s1 = serialport("/dev/ttyUSB0", 9600)
flush(s1);
y_temp = cell(10,1)
y = 0
while true
for i = 1:10
y_serial = str2num(char(fread(s1,8)))
y_temp {i,1} = y_serial
endfor
y = cat(1, y, y_temp{1:10})
plot(y)
pause(1)
endwhile
srl_close(s1)
This works... so long as the number of bytes per string is 8. How can I make this dynamic and split by line endings?
All I need to do is plot incrementing by 1 x_value and a float y_value from serial.
https://www.edn.com/read-serial-data-directly-into-octave/
Solution found.
Their is no inbuilt solution but you can make your own function like:
function [char_array] = ReadToTermination (srl_handle, term_char)
% parameter term_char is optional, if not specified
% then CR = 'r' = 13dec is the default.
if (nargin == 1)
term_char = 13;
end
not_terminated = true;
i = 1;
int_array = uint8(1);
while not_terminated
val = fread(srl_handle, 1);
if(val == term_char)
not_terminated = false;
end
% Add char received to array
int_array(i) = val;
i = i + 1;
end
% Change int array to a char array and return a string array
char_array = char(int_array);
endfunction
This does not actually work straight off and I had to add \n to my Arduino script.
disp('load instrument control:');
pkg load instrument-control
disp('SerialPort:');
serialportlist
cpx = serialport ("COM6", 57600);
disp(cpx);
disp(sprintf('InBuffer:%3d', cpx.NumBytesAvailable));
if (cpx.NumBytesAvailable > 0)
zx = read(cpx, cpx.NumBytesAvailable);
disp(sprintf("IN:\r\n%s", zx));
endif;

How to create different iterations of 6 digit integer that is also 6 digits?

I am using an algorithm to create a 6 digit pin from a string of letters(I already have it). I also need to make different iterations of this 6 digit pin that would all lead back to the origin pin which can be used to generate the string of letters.
input "FEFOEISUDFRORI"
output 523923
some algorithm...
first iteration: 123203
then to authenticate
iteration: 1 ; pin: 123203
output: 'FEFOEISUDFRORI' // same as original string
Any idea how to do this?
The easiest way to solve this mathematical problem is probably with a rotation. Essentially performing an addition then a modulus, rotation will result in a one-to-one function with a range equal to it's domain.
The example I've shown will rotate the entire 6 digit number or the individual digits of the number.
function rRot(x, rot, max) {
if (rot < 0) return lRot(x,-rot,max);
rot = rot % max;
return (x + rot) % max;
}
function lRot(x, rot, max) {
if (rot < 0) return rRot(x,-rot,max);
rot = rot % max;
return rRot(x,max-rot,max);
}
function rotDigits(x, r) {
var pwr = 1, y = 0;
while (x > 0) {
var digit = x % 10;
y += rRot(digit, r, 10) * pwr;
x = Math.floor(x / 10);
pwr *= 10;
}
return y;
}
var samples = [675821, 126421, 678321, 100001, 580127, 999999];
(function () {
console.log("Rotate individual digits");
samples.forEach(v => {
var r = rotDigits(v, 7);
var vr = rotDigits(r, 10-7);
console.log(v.toString() + " => " + r.toString() + " => " + vr.toString());
});
console.log("Rotate whole number");
samples.forEach(v => {
var r = rRot(v, 65537, 1000000);
var vr = lRot(r, 65537, 1000000);
console.log(v.toString() + " => " + r.toString() + " => " + vr.toString());
});
})()

Create a GTFS realtime (vehicle positions) with R

I work on turning a dataframe into a GTFS realtime, and am struggling on the vehicle position part.
My data looks like that (stored in a dataframe called "vehicle"):
## Input data looks that way, one line per on-going vehicle
vehicle_id trip_id lat lon bear speed stop_time
52108 4.264930e+05 45.40 -71.92 1 9 2017-05-02 15:19:05
60105 4.273610e+05 45.40 -71.90 246 6 2017-05-02 15:18:59
59104 4.270150e+05 45.40 -71.87 81 7 2017-05-02 15:18:54
The details of my code is:
library(dplyr)
library(XML)
library(stringr)
library(RProtoBuf)
library(RODBC)
## Read the google gtfs proto file
readProtoFiles("gtfs-realtime.proto")
## List of current vehicles
current_vehicles <- unique(vehicle$vehicle_id)
## Create an empty list, 1 entry for each vehicle
protobuf_list <- vector(mode = "list", length = length(current_vehicles))
## Loop over all current vehicles
for(i in 1:length(current_vehicles)) {
## protobuf object
vehicle_position_update <- new(transit_realtime.VehiclePosition,
vehicle = vehicle$vehicle_id[i],
stop_id = vehicle$stop_id[i],
trip = vehicle$trip_id[i],
latitude = vehicle$lat[i],
longitude = vehicle$lon[i],
bearing = vehicle$bear[i],
speed = vehicle$speed[i])
## protobuf feed entity
e <- new(transit_realtime.FeedEntity,
id = as.character(vehicle$vehicle_id[i]),
vehicle = new(transit_realtime.VehiclePosition,
trip = new(transit_realtime.VehicleDescriptor,
id = vehicle$vehicle_id[i]),
VehiclePosition = vehicle_position_update))
## Fill the list
protobuf_list[[i]] <- e
}# Loop over vehicles
## GTFS header
header_object <- new(transit_realtime.FeedHeader,
gtfs_realtime_version = "1.0",
incrementality = "FULL_DATASET",
timestamp = as.numeric(as.POSIXlt(Sys.time())))
## Build the full GTFS
m <- new(transit_realtime.FeedMessage,
header = header_object,
entity = protobuf_list) # use entity_list
## Write the GTFS
writeLines(as.character(m))
## Turn it into binary
serialize(m, "vehiclePositions.pb")
When creating the protobuffer object vehicle_position_update, it crashes with the message:
type mismatch, expecting a 'Message' object
I went through the gtfs-realtime.proto, and my understanding of the different messages to include seems fine (well, obviously it'nt..).
Does anyone see why this protobuffer file cannot be created?
ADDED FOR A CLEAR SOLUTION:
My issue was that I was'nt following exactly the gtfs proto descriptions of the different messages. Once this point corrected, the loop over the vehicles becomes:
## Loop over all current vehicles
for(i in 1:length(current_vehicles)) {
## protobuf object
vehicle_position_update <- new(transit_realtime.Position,
latitude = vehicle$lat[i],
longitude = vehicle$lon[i],
bearing = vehicle$bear[i],
speed = vehicle$speed[i])
## protobuf feed entity
e <- new(transit_realtime.FeedEntity,
id = as.character(vehicle$vehicle_id[i]),
vehicle = new(transit_realtime.VehiclePosition,
trip = new(transit_realtime.TripDescriptor,
trip_id = vehicle$trip_id[i],
route_id = vehicle$route_id[i]),
stop_id = vehicle$stop_id[i],
position = vehicle_position_update))
## Fill the list
protobuf_list[[i]] <- e
}# Loop over vehicles
and it works
The message definition tells you what fields it requires, for example
writeLines(as.character(RProtoBuf::fileDescriptor(transit_realtime.FeedMessage)))
message FeedMessage {
required .transit_realtime.FeedHeader header = 1;
repeated .transit_realtime.FeedEntity entity = 2;
extensions 1000 to 1999;
}
message FeedHeader {
enum Incrementality {
FULL_DATASET = 0;
DIFFERENTIAL = 1;
}
required string gtfs_realtime_version = 1;
optional .transit_realtime.FeedHeader.Incrementality incrementality = 2 [default = FULL_DATASET];
optional uint64 timestamp = 3;
extensions 1000 to 1999;
}
message FeedEntity {
required string id = 1;
optional bool is_deleted = 2 [default = false];
optional .transit_realtime.TripUpdate trip_update = 3;
optional .transit_realtime.VehiclePosition vehicle = 4;
optional .transit_realtime.Alert alert = 5;
extensions 1000 to 1999;
}
... etc
Then, if you take a look at the Position message, you see the fields
message Position {
required float latitude = 1;
required float longitude = 2;
optional float bearing = 3;
optional double odometer = 4;
optional float speed = 5;
extensions 1000 to 1999;
}
So you define the Position using those values, e.g.
RProtoBuf::new(transit_realtime.Position, latitude = 0, longitude = 0)
And the VehiclePosition message is
message VehiclePosition {
enum VehicleStopStatus {
INCOMING_AT = 0;
STOPPED_AT = 1;
IN_TRANSIT_TO = 2;
}
enum CongestionLevel {
UNKNOWN_CONGESTION_LEVEL = 0;
RUNNING_SMOOTHLY = 1;
STOP_AND_GO = 2;
CONGESTION = 3;
SEVERE_CONGESTION = 4;
}
enum OccupancyStatus {
EMPTY = 0;
MANY_SEATS_AVAILABLE = 1;
FEW_SEATS_AVAILABLE = 2;
STANDING_ROOM_ONLY = 3;
CRUSHED_STANDING_ROOM_ONLY = 4;
FULL = 5;
NOT_ACCEPTING_PASSENGERS = 6;
}
optional .transit_realtime.TripDescriptor trip = 1;
optional .transit_realtime.VehicleDescriptor vehicle = 8;
optional .transit_realtime.Position position = 2;
optional uint32 current_stop_sequence = 3;
optional string stop_id = 7;
optional .transit_realtime.VehiclePosition.VehicleStopStatus current_status = 4 [default = IN_TRANSIT_TO];
optional uint64 timestamp = 5;
optional .transit_realtime.VehiclePosition.CongestionLevel congestion_level = 6;
optional .transit_realtime.VehiclePosition.OccupancyStatus occupancy_status = 9;
extensions 1000 to 1999;
}
So the message will be like
RProtoBuf::new(transit_realtime.VehiclePosition,
current_status = 1,
congestion_level = 0,
stop_id = "7",
current_stop_sequence = 1)

Find closest value in a vector with binary search

As a silly toy example, suppose
x=4.5
w=c(1,2,4,6,7)
I wonder if there is a simple R function that finds the index of the closest match to x in w. So if foo is that function, foo(w,x) would return 3. The function match is the right idea, but seems to apply only for exact matches.
Solutions here (e.g. which.min(abs(w - x)), which(abs(w-x)==min(abs(w-x))), etc.) are all O(n) instead of log(n) (I'm assuming that w is already sorted).
R>findInterval(4.5, c(1,2,4,5,6))
[1] 3
will do that with price-is-right matching (closest without going over).
You can use data.table to do a binary search:
dt = data.table(w, val = w) # you'll see why val is needed in a sec
setattr(dt, "sorted", "w") # let data.table know that w is sorted
Note that if the column w isn't already sorted, then you'll have to use setkey(dt, w) instead of setattr(.).
# binary search and "roll" to the nearest neighbour
dt[J(x), roll = "nearest"]
# w val
#1: 4.5 4
In the final expression the val column will have the you're looking for.
# or to get the index as Josh points out
# (and then you don't need the val column):
dt[J(x), .I, roll = "nearest", by = .EACHI]
# w .I
#1: 4.5 3
# or to get the index alone
dt[J(x), roll = "nearest", which = TRUE]
#[1] 3
See match.closest() from the MALDIquant package:
> library(MALDIquant)
> match.closest(x, w)
[1] 3
x = 4.5
w = c(1,2,4,6,7)
closestLoc = which(min(abs(w-x)))
closestVal = w[which(min(abs(w-x)))]
# On my phone- please pardon typos
If your vector is lengthy, try a 2-step approach:
x = 4.5
w = c(1,2,4,6,7)
sdev = sapply(w,function(v,x) abs(v-x), x = x)
closestLoc = which(min(sdev))
for maddeningly long vectors (millions of rows!, warning- this will actually be slower for data which is not very, very, very large.)
require(doMC)
registerDoMC()
closestLoc = which(min(foreach(i = w) %dopar% {
abs(i-x)
}))
This example is just to give you a basic idea of leveraging parallel processing when you have huge data. Note, I do not recommend you use it for simple & fast functions like abs().
To do this on character vectors, Martin Morgan suggested this function on R-help:
bsearch7 <-
function(val, tab, L=1L, H=length(tab))
{
b <- cbind(L=rep(L, length(val)), H=rep(H, length(val)))
i0 <- seq_along(val)
repeat {
updt <- M <- b[i0,"L"] + (b[i0,"H"] - b[i0,"L"]) %/% 2L
tabM <- tab[M]
val0 <- val[i0]
i <- tabM < val0
updt[i] <- M[i] + 1L
i <- tabM > val0
updt[i] <- M[i] - 1L
b[i0 + i * length(val)] <- updt
i0 <- which(b[i0, "H"] >= b[i0, "L"])
if (!length(i0)) break;
}
b[,"L"] - 1L
}
NearestValueSearch = function(x, w){
## A simple binary search algo
## Assume the w vector is sorted so we can use binary search
left = 1
right = length(w)
while(right - left > 1){
middle = floor((left + right) / 2)
if(x < w[middle]){
right = middle
}
else{
left = middle
}
}
if(abs(x - w[right]) < abs(x - w[left])){
return(right)
}
else{
return(left)
}
}
x = 4.5
w = c(1,2,4,6,7)
NearestValueSearch(x, w) # return 3
Based on #neal-fultz answer, here is a simple function that uses findInterval():
get_closest_index <- function(x, vec){
# vec must be sorted
iv <- findInterval(x, vec)
dist_left <- x - vec[ifelse(iv == 0, NA, iv)]
dist_right <- vec[iv + 1] - x
ifelse(! is.na(dist_left) & (is.na(dist_right) | dist_left < dist_right), iv, iv + 1)
}
values <- c(-15, -0.01, 3.1, 6, 10, 100)
grid <- c(-2, -0.1, 0.1, 3, 7)
get_closest_index(values, grid)
#> [1] 1 2 4 5 5 5
Created on 2020-05-29 by the reprex package (v0.3.0)
You can always implement custom binary search algorithm to find the closest value. Alternately, you can leverage standard implementation of libc bsearch(). You can use other binary search implementations as well, but it does not change the fact that you have to implement the comparing function carefully to find the closest element in array. The issue with standard binary search implementation is that it is meant for exact comparison. That means your improvised comparing function needs to do some kind of exactification to figure out if an element in array is close-enough. To achieve it, the comparing function needs to have awareness of other elements in the array, especially following aspects:
position of the current element (one which is being compared with the
key).
the distance with key and how it compares with neighbors (previous
or next element).
To provide this extra knowledge in comparing function, the key needs to be packaged with additional information (not just the key value). Once the comparing function have awareness on these aspects, it can figure out if the element itself is closest. When it knows that it is the closest, it returns "match".
The the following C code finds the closest value.
#include <stdio.h>
#include <stdlib.h>
struct key {
int key_val;
int *array_head;
int array_size;
};
int compar(const void *k, const void *e) {
struct key *key = (struct key*)k;
int *elem = (int*)e;
int *arr_first = key->array_head;
int *arr_last = key->array_head + key->array_size -1;
int kv = key->key_val;
int dist_left;
int dist_right;
if (kv == *elem) {
/* easy case: if both same, got to be closest */
return 0;
} else if (key->array_size == 1) {
/* easy case: only element got to be closest */
return 0;
} else if (elem == arr_first) {
/* element is the first in array */
if (kv < *elem) {
/* if keyval is less the first element then
* first elem is closest.
*/
return 0;
} else {
/* check distance between first and 2nd elem.
* if distance with first elem is smaller, it is closest.
*/
dist_left = kv - *elem;
dist_right = *(elem+1) - kv;
return (dist_left <= dist_right) ? 0:1;
}
} else if (elem == arr_last) {
/* element is the last in array */
if (kv > *elem) {
/* if keyval is larger than the last element then
* last elem is closest.
*/
return 0;
} else {
/* check distance between last and last-but-one.
* if distance with last elem is smaller, it is closest.
*/
dist_left = kv - *(elem-1);
dist_right = *elem - kv;
return (dist_right <= dist_left) ? 0:-1;
}
}
/* condition for remaining cases (other cases are handled already):
* - elem is neither first or last in the array
* - array has atleast three elements.
*/
if (kv < *elem) {
/* keyval is smaller than elem */
if (kv <= *(elem -1)) {
/* keyval is smaller than previous (of "elem") too.
* hence, elem cannot be closest.
*/
return -1;
} else {
/* check distance between elem and elem-prev.
* if distance with elem is smaller, it is closest.
*/
dist_left = kv - *(elem -1);
dist_right = *elem - kv;
return (dist_right <= dist_left) ? 0:-1;
}
}
/* remaining case: (keyval > *elem) */
if (kv >= *(elem+1)) {
/* keyval is larger than next (of "elem") too.
* hence, elem cannot be closest.
*/
return 1;
}
/* check distance between elem and elem-next.
* if distance with elem is smaller, it is closest.
*/
dist_right = *(elem+1) - kv;
dist_left = kv - *elem;
return (dist_left <= dist_right) ? 0:1;
}
int main(int argc, char **argv) {
int arr[] = {10, 20, 30, 40, 50, 60, 70};
int *found;
struct key k;
if (argc < 2) {
return 1;
}
k.key_val = atoi(argv[1]);
k.array_head = arr;
k.array_size = sizeof(arr)/sizeof(int);
found = (int*)bsearch(&k, arr, sizeof(arr)/sizeof(int), sizeof(int),
compar);
if(found) {
printf("found closest: %d\n", *found);
} else {
printf("closest not found. absurd! \n");
}
return 0;
}
Needless to say that bsearch() in above example should never fail (unless the array size is zero).
If you implement your own custom binary search, essentially you have to embed same comparing logic in the main body of binary search code (instead of having this logic in comparing function in above example).

Kinect skeleton Scaling strange behaviour

I am trying to scale a skeleton to match to the sizes of another skeleton.
My algoritm do the following:
Find the distance between two joints of the origin skeleton and the destiny skeleton using phytagorean teorem
divide this two distances to find a multiply factor.
Multiply each joint by this factor.
Here is my actual code:
public static Skeleton ScaleToMatch(this Skeleton skToBeScaled, Skeleton skDestiny)
{
Joint newJoint = new Joint();
double distanciaOrigem = 0;
double distanciaDestino = 0;
double fator = 1;
SkeletonPoint pos = new SkeletonPoint();
foreach (BoneOrientation bo in skToBeScaled.BoneOrientations)
{
distanciaOrigem = FisioKinectCalcs.Distance3DBetweenJoint(skToBeScaled.Joints[bo.StartJoint], skToBeScaled.Joints[bo.EndJoint]);
distanciaDestino = FisioKinectCalcs.Distance3DBetweenJoint(skDestiny.Joints[bo.StartJoint], skDestiny.Joints[bo.EndJoint]);
if (distanciaOrigem > 0 && distanciaDestino > 0)
{
fator = (distanciaDestino / distanciaOrigem);
newJoint = skToBeScaled.Joints[bo.EndJoint]; // escaling only the end joint as the BoneOrientatios starts from HipCenter, i am scaling from center to edges.
// applying the new values to the joint
pos = new SkeletonPoint()
{
X = (float)(newJoint.Position.X * fator),
Y = (float)(newJoint.Position.Y * fator),
Z = (float)(newJoint.Position.Z * fator)
};
newJoint.Position = pos;
skToBeScaled.Joints[bo.EndJoint] = newJoint;
}
}
return skToBeScaled;
}
Every seems to work fine except for the hands and foots
Look at this images
I have my own skeleton over me, and my skeleton scaled to the sizes of another person, but the hands and foots still crazy. (but code looks right)
Any suggestion?
It's hard to say without running the code, but it somewhat "looks good".
What I would validate though, is your
if (distanciaOrigem > 0 && distanciaDestino > 0)
If distanciaOrigem is very close to 0, but even just epsilon away from 0, it won't be picked up by the if, and then
fator = (distanciaDestino / distanciaOrigem);
Will result in a very large number!
I would suggest to smooth the factor so it generally fits the proper scale. Try this code:
private static Dictionary<JointType, double> jointFactors = null;
static CalibrationUtils()
{
InitJointFactors();
}
public static class EnumUtil
{
public static IEnumerable<T> GetValues<T>()
{
return Enum.GetValues(typeof(T)).Cast<T>();
}
}
private static void InitJointFactors()
{
var jointTypes = EnumUtil.GetValues<JointType>();
jointFactors = new Dictionary<JointType, double>();
foreach(JointType type in jointTypes)
{
jointFactors.Add(type, 0);
}
}
private static double SmoothenFactor(JointType jointType, double factor, int weight)
{
double currentValue = jointFactors[jointType];
double newValue = 0;
if(currentValue != 0)
newValue = (weight * currentValue + factor) / (weight + 1);
else
newValue = factor;
jointFactors[jointType] = newValue;
return newValue;
}
When it comes to factor usage just use the SmoothenFactor method first:
public static Skeleton ScaleToMatch(this Skeleton skToBeScaled, Skeleton skDestiny, double additionalFactor = 1)
{
Joint newJoint = new Joint();
double distanceToScale = 0;
double distanceDestiny = 0;
double factor = 1;
int weight = 500;
SkeletonPoint pos = new SkeletonPoint();
Skeleton newSkeleton = null;
KinectHelper.CopySkeleton(skToBeScaled, ref newSkeleton);
SkeletonPoint hipCenterPosition = newSkeleton.Joints[JointType.HipCenter].Position;
foreach(BoneOrientation bo in skToBeScaled.BoneOrientations)
{
distanceToScale = Distance3DBetweenJoints(skToBeScaled.Joints[bo.StartJoint], skToBeScaled.Joints[bo.EndJoint]);
distanceDestiny = Distance3DBetweenJoints(skDestiny.Joints[bo.StartJoint], skDestiny.Joints[bo.EndJoint]);
if(distanceToScale > 0 && distanceDestiny > 0)
{
factor = (distanceDestiny / distanceToScale) * additionalFactor;
newJoint = skToBeScaled.Joints[bo.EndJoint]; // escaling only the end joint as the BoneOrientatios starts from HipCenter, i am scaling from center to edges.
factor = SmoothenFactor(newJoint.JointType, factor, weight);
pos = new SkeletonPoint()
{
X = (float)((newJoint.Position.X - hipCenterPosition.X) * factor + hipCenterPosition.X),
Y = (float)((newJoint.Position.Y - hipCenterPosition.Y) * factor + hipCenterPosition.Y),
Z = (float)((newJoint.Position.Z - hipCenterPosition.Z) * factor + hipCenterPosition.Z)
};
newJoint.Position = pos;
newSkeleton.Joints[bo.EndJoint] = newJoint;
}
}
return newSkeleton;
}
I also modified your ScaleToMatch method as you see. There was a need to move joints in relation to HipCenter position. Also new positions are saved to a new Skeleton instance so they are not used in further vector calculations.
Experiment with the weight but since our bones length is constant you can use big numbers like 100 and more to be sure that wrong Kinect readings do not disturb the correct scale.
Here's an example of how it helped with scaling HandRight joint position:
The weight was set to 500. The resulting factor is supposed to be around 2 (because the base skeleton was purposely downscaled by a factor of 2).
I hope it helps!

Resources